diff --git a/wrap/tests/cythontest.h b/wrap/tests/cythontest.h deleted file mode 100644 index 8bf097153..000000000 --- a/wrap/tests/cythontest.h +++ /dev/null @@ -1,2662 +0,0 @@ -namespace gtsam { - -#include -typedef size_t Key; - -#include -template class FastVector { - FastVector(); - FastVector(const This& f); -}; - -typedef gtsam::FastVector KeyVector; - -#include -template class FastList { - FastList(); - FastList(const This& f); -}; -typedef gtsam::FastList KeyList; - -#include -template class FastSet { - FastSet(); - FastSet(const This& f); -}; -typedef gtsam::FastSet KeySet; - -#include -template class FastMap { - FastMap(); - FastMap(const This& f); -}; - -#include -virtual class Value { - // No constructors because this is an abstract class - - // Testable - void print(string s) const; - - // Manifold - size_t dim() const; -}; - -#include -class LieScalar { - // Standard constructors - LieScalar(); - LieScalar(const gtsam::LieScalar& l); - LieScalar(double d); - - // Standard interface - double value() const; - - // Testable - void print(string s) const; - bool equals(const gtsam::LieScalar& expected, double tol) const; - - // Group - static gtsam::LieScalar identity(); - gtsam::LieScalar inverse() const; - gtsam::LieScalar compose(const gtsam::LieScalar& p) const; - gtsam::LieScalar between(const gtsam::LieScalar& l2) const; - - // Manifold - size_t dim() const; - gtsam::LieScalar retract(Vector v) const; - Vector localCoordinates(const gtsam::LieScalar& t2) const; - - // Lie group - static gtsam::LieScalar Expmap(Vector v); - static Vector Logmap(const gtsam::LieScalar& p); -}; - -#include -class LieVector { - // Standard constructors - LieVector(); - LieVector(const gtsam::LieVector& l); - LieVector(Vector v); - - // Standard interface - Vector vector() const; - - // Testable - void print(string s) const; - bool equals(const gtsam::LieVector& expected, double tol) const; - - // Group - static gtsam::LieVector identity(); - gtsam::LieVector inverse() const; - gtsam::LieVector compose(const gtsam::LieVector& p) const; - gtsam::LieVector between(const gtsam::LieVector& l2) const; - - // Manifold - size_t dim() const; - gtsam::LieVector retract(Vector v) const; - Vector localCoordinates(const gtsam::LieVector& t2) const; - - // Lie group - static gtsam::LieVector Expmap(Vector v); - static Vector Logmap(const gtsam::LieVector& p); - - // enabling serialization functionality - void serialize() const; -}; - -#include -class LieMatrix { - // Standard constructors - LieMatrix(); - LieMatrix(Matrix v); - LieMatrix(const gtsam::LieMatrix& l); - - // Standard interface - Matrix matrix() const; - - // Testable - void print(string s) const; - bool equals(const gtsam::LieMatrix& expected, double tol) const; - - // Group - static gtsam::LieMatrix identity(); - gtsam::LieMatrix inverse() const; - gtsam::LieMatrix compose(const gtsam::LieMatrix& p) const; - gtsam::LieMatrix between(const gtsam::LieMatrix& l2) const; - - // Manifold - size_t dim() const; - gtsam::LieMatrix retract(Vector v) const; - Vector localCoordinates(const gtsam::LieMatrix & t2) const; - - // Lie group - static gtsam::LieMatrix Expmap(Vector v); - static Vector Logmap(const gtsam::LieMatrix& p); - - // enabling serialization functionality - void serialize() const; -}; - -//************************************************************************* -// geometry -//************************************************************************* - -#include -class Point2 { - // Standard Constructors - Point2(); - Point2(double x, double y); - Point2(Vector v); - Point2(const gtsam::Point2& l); - - // Testable - void print(string s) const; - bool equals(const gtsam::Point2& pose, 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; -}; - -// std::vector -#include -class Point2Vector -{ - // Constructors - Point2Vector(); - Point2Vector(const gtsam::Point2Vector& v); - - //Capacity - size_t size() const; - size_t max_size() const; - void resize(size_t sz); - size_t capacity() const; - bool empty() const; - void reserve(size_t n); - - //Element access - gtsam::Point2 at(size_t n) const; - gtsam::Point2 front() const; - gtsam::Point2 back() const; - - //Modifiers - void assign(size_t n, const gtsam::Point2& u); - void push_back(const gtsam::Point2& x); - void pop_back(); -}; - -#include -class StereoPoint2 { - // Standard Constructors - StereoPoint2(); - StereoPoint2(double uL, double uR, double v); - StereoPoint2(const gtsam::StereoPoint2& l); - - // Testable - void print(string s) const; - bool equals(const gtsam::StereoPoint2& point, double tol) const; - - // Group - static gtsam::StereoPoint2 identity(); - gtsam::StereoPoint2 inverse() const; - gtsam::StereoPoint2 compose(const gtsam::StereoPoint2& p2) const; - gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2) const; - - // Manifold - gtsam::StereoPoint2 retract(Vector v) const; - Vector localCoordinates(const gtsam::StereoPoint2& p) const; - - // Lie Group - static gtsam::StereoPoint2 Expmap(Vector v); - static Vector Logmap(const gtsam::StereoPoint2& p); - - // Standard Interface - Vector vector() const; - double uL() const; - double uR() const; - double v() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -class Point3 { - // Standard Constructors - Point3(); - Point3(double x, double y, double z); - Point3(Vector v); - Point3(const gtsam::Point3& l); - - // Testable - void print(string s) const; - bool equals(const gtsam::Point3& p, double tol) const; - - // Group - static gtsam::Point3 identity(); - - // Standard Interface - Vector vector() const; - double x() const; - double y() const; - double z() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -class Rot2 { - // Standard Constructors and Named Constructors - Rot2(); - Rot2(double theta); - Rot2(const gtsam::Rot2& l); - - 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) 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; - - // 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); - - // 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; -}; - -#include -class Rot3 { - // Standard Constructors and Named Constructors - Rot3(); - Rot3(Matrix R); - Rot3(const gtsam::Rot3& l); - - static gtsam::Rot3 Rx(double t); - static gtsam::Rot3 Ry(double t); - static gtsam::Rot3 Rz(double t); - static gtsam::Rot3 RzRyRx(double x, double y, double z); - static gtsam::Rot3 RzRyRx(Vector xyz); - static gtsam::Rot3 Yaw(double t); // positive yaw is to right (as in aircraft heading) - static gtsam::Rot3 Pitch(double t); // positive pitch is up (increasing aircraft altitude) - static gtsam::Rot3 Roll(double t); // positive roll is to right (increasing yaw in aircraft) - static gtsam::Rot3 Ypr(double y, double p, double r); - static gtsam::Rot3 Quaternion(double w, double x, double y, double z); - static gtsam::Rot3 Rodrigues(Vector v); - - // Testable - void print(string s) const; - bool equals(const gtsam::Rot3& rot, double tol) const; - - // Group - static gtsam::Rot3 identity(); - gtsam::Rot3 inverse() const; - gtsam::Rot3 compose(const gtsam::Rot3& p2) const; - gtsam::Rot3 between(const gtsam::Rot3& p2) const; - - // Manifold - //gtsam::Rot3 retractCayley(Vector v) const; // FIXME, does not exist in both Matrix and Quaternion options - gtsam::Rot3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Rot3& p) const; - - // Group Action on Point3 - gtsam::Point3 rotate(const gtsam::Point3& p) const; - gtsam::Point3 unrotate(const gtsam::Point3& p) const; - - // Standard Interface - static gtsam::Rot3 Expmap(Vector v); - static Vector Logmap(const gtsam::Rot3& p); - Matrix matrix() const; - Matrix transpose() const; - gtsam::Point3 column(size_t index) const; - Vector xyz() const; - Vector ypr() const; - Vector rpy() const; - double roll() const; - double pitch() const; - double yaw() const; -// Vector toQuaternion() const; // FIXME: Can't cast to Vector properly - Vector quaternion() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -class Pose2 { - // Standard Constructor - Pose2(); - Pose2(const gtsam::Pose2& pose); - Pose2(double x, double y, double theta); - Pose2(double theta, const gtsam::Point2& t); - Pose2(const gtsam::Rot2& r, const gtsam::Point2& t); - Pose2(Vector v); - - // Testable - void print(string s) const; - bool equals(const gtsam::Pose2& pose, double tol) const; - - // Group - static gtsam::Pose2 identity(); - gtsam::Pose2 inverse() const; - gtsam::Pose2 compose(const gtsam::Pose2& p2) const; - gtsam::Pose2 between(const gtsam::Pose2& p2) const; - - // Manifold - gtsam::Pose2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Pose2& p) const; - - // Lie Group - static gtsam::Pose2 Expmap(Vector v); - static Vector Logmap(const gtsam::Pose2& p); - Matrix AdjointMap() const; - Vector Adjoint(const Vector& xi) const; - static Matrix wedge(double vx, double vy, double w); - - // Group Actions on Point2 - gtsam::Point2 transform_from(const gtsam::Point2& p) const; - gtsam::Point2 transform_to(const gtsam::Point2& p) const; - - // Standard Interface - double x() const; - double y() const; - double theta() const; - gtsam::Rot2 bearing(const gtsam::Point2& point) const; - double range(const gtsam::Point2& point) const; - gtsam::Point2 translation() const; - gtsam::Rot2 rotation() const; - Matrix matrix() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -class Pose3 { - // Standard Constructors - Pose3(); - Pose3(const gtsam::Pose3& pose); - Pose3(const gtsam::Rot3& r, const gtsam::Point3& t); - Pose3(const gtsam::Pose2& pose2); // FIXME: shadows Pose3(Pose3 pose) - Pose3(Matrix t); - - // 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& p2) const; - gtsam::Pose3 between(const gtsam::Pose3& p2) const; - - // Manifold - gtsam::Pose3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Pose3& T2) const; - - // Lie Group - static gtsam::Pose3 Expmap(Vector v); - static Vector Logmap(const gtsam::Pose3& p); - Matrix AdjointMap() const; - Vector Adjoint(Vector xi) const; - static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz); - - // Group Action on Point3 - gtsam::Point3 transform_from(const gtsam::Point3& p) const; - gtsam::Point3 transform_to(const gtsam::Point3& p) 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 transform_to(const gtsam::Pose3& pose) const; // FIXME: shadows other transform_to() - double range(const gtsam::Point3& point); - double range(const gtsam::Pose3& pose); - - // enabling serialization functionality - void serialize() const; -}; - -// std::vector -#include -class Pose3Vector -{ - Pose3Vector(); - Pose3Vector(const gtsam::Pose3Vector& l); - - size_t size() const; - bool empty() const; - gtsam::Pose3 at(size_t n) const; - void push_back(const gtsam::Pose3& x); -}; - -#include -class Unit3 { - // Standard Constructors - Unit3(); - Unit3(const gtsam::Point3& pose); - Unit3(const gtsam::Unit3& pose); - - // Testable - void print(string s) const; - bool equals(const gtsam::Unit3& pose, double tol) const; - - // Other functionality - Matrix basis() const; - Matrix skew() const; - - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Unit3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Unit3& s) const; -}; - -#include -class EssentialMatrix { - EssentialMatrix(); - // Standard Constructors - EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb); - EssentialMatrix(const gtsam::EssentialMatrix& pose); - - // 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); - Cal3_S2(const gtsam::Cal3_S2& pose); - - // Testable - void print(string s) const; - bool equals(const gtsam::Cal3_S2& rhs, double tol) const; - - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Cal3_S2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3_S2& c) const; - - // Action on Point2 - gtsam::Point2 calibrate(const gtsam::Point2& p) const; - gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; - - // Standard Interface - double fx() const; - double fy() const; - double skew() const; - double px() const; - double py() const; - gtsam::Point2 principalPoint() const; - Vector vector() const; - Matrix matrix() const; - Matrix matrix_inverse() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -virtual class Cal3DS2_Base { - // Standard Constructors - Cal3DS2_Base(); - Cal3DS2_Base(const gtsam::Cal3DS2_Base& pose); - - // Testable - void print(string s) const; - - // Standard Interface - double fx() const; - double fy() const; - double skew() const; - double px() const; - double py() const; - double k1() const; - double k2() const; - - // Action on Point2 - gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; - gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const; - // gtsam::Point2 calibrate(const gtsam::Point2& p) const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -virtual class Cal3DS2 : gtsam::Cal3DS2_Base { - // Standard Constructors - Cal3DS2(); - Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2); - Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2); - Cal3DS2(Vector v); - Cal3DS2(const gtsam::Cal3DS2& pose); - - // Testable - bool equals(const gtsam::Cal3DS2& rhs, double tol) const; - - // Manifold - size_t dim() const; - static size_t Dim(); - gtsam::Cal3DS2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3DS2& c) const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -virtual class Cal3Unified : gtsam::Cal3DS2_Base { - // Standard Constructors - Cal3Unified(); - Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2); - Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2, double xi); - Cal3Unified(Vector v); - Cal3Unified(const gtsam::Cal3Unified& pose); - - // Testable - bool equals(const gtsam::Cal3Unified& rhs, double tol) const; - - // Standard Interface - double xi() const; - gtsam::Point2 spaceToNPlane(const gtsam::Point2& p) const; - gtsam::Point2 nPlaneToSpace(const gtsam::Point2& p) const; - - // Manifold - size_t dim() const; - static size_t Dim(); - gtsam::Cal3Unified retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3Unified& c) const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -class Cal3_S2Stereo { - // Standard Constructors - Cal3_S2Stereo(); - Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b); - Cal3_S2Stereo(Vector v); - Cal3_S2Stereo(const gtsam::Cal3_S2Stereo& pose); - - // 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(const gtsam::Cal3Bundler& pose); - - // Testable - void print(string s) const; - bool equals(const gtsam::Cal3Bundler& rhs, double tol) const; - - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Cal3Bundler retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3Bundler& c) const; - - // Action on Point2 - gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const; - gtsam::Point2 calibrate(const gtsam::Point2& p) const; - gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; - - // Standard Interface - double fx() const; - double fy() const; - double k1() const; - double k2() const; - double u0() const; - double v0() const; - Vector vector() const; - Vector k() const; - //Matrix K() const; //FIXME: Uppercase - - // enabling serialization functionality - void serialize() const; -}; - -#include -class CalibratedCamera { - // Standard Constructors and Named Constructors - CalibratedCamera(); - CalibratedCamera(const gtsam::Pose3& pose); - CalibratedCamera(const Vector& v); - static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height); - CalibratedCamera(const gtsam::CalibratedCamera& pose); - - // Testable - void print(string s) const; - bool equals(const gtsam::CalibratedCamera& camera, double tol) const; - - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::CalibratedCamera retract(const Vector& d) const; - Vector localCoordinates(const gtsam::CalibratedCamera& T2) const; - - // Action on Point3 - gtsam::Point2 project(const gtsam::Point3& point) const; - static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); - - // Standard Interface - gtsam::Pose3 pose() const; - double range(const gtsam::Point3& p) const; // TODO: Other overloaded range methods - - // enabling serialization functionality - void serialize() const; -}; - -#include -template -class PinholeCamera { - // Standard Constructors and Named Constructors - PinholeCamera(); - PinholeCamera(const This& cam); - PinholeCamera(const gtsam::Pose3& pose); - PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K); - static This Level(const CALIBRATION& K, const gtsam::Pose2& pose, double height); - static This Level(const gtsam::Pose2& pose, double height); - static This Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, - const gtsam::Point3& upVector, const CALIBRATION& K); - - // Testable - void print(string s) const; - bool equals(const This& camera, double tol) const; - - // Standard Interface - gtsam::Pose3 pose() const; - CALIBRATION calibration() const; - - // Manifold - This retract(const Vector& d) const; - Vector localCoordinates(const This& T2) const; - size_t dim() const; - static size_t Dim(); - - // Transformations and measurement functions - static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); - pair projectSafe(const gtsam::Point3& pw) const; - gtsam::Point2 project(const gtsam::Point3& point); - gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; - double range(const gtsam::Point3& point); - double range(const gtsam::Pose3& point); - - // enabling serialization functionality - void serialize() const; -}; - -#include -virtual class SimpleCamera { - // Standard Constructors and Named Constructors - SimpleCamera(); - SimpleCamera(const gtsam::Pose3& pose); - SimpleCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2& K); - static gtsam::SimpleCamera Level(const gtsam::Cal3_S2& K, const gtsam::Pose2& pose, double height); - static gtsam::SimpleCamera Level(const gtsam::Pose2& pose, double height); - static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, - const gtsam::Point3& upVector, const gtsam::Cal3_S2& K); - SimpleCamera(const gtsam::SimpleCamera& other); - - // Testable - void print(string s) const; - bool equals(const gtsam::SimpleCamera& camera, double tol) const; - - // Standard Interface - gtsam::Pose3 pose() const; - gtsam::Cal3_S2 calibration() const; - - // Manifold - gtsam::SimpleCamera retract(const Vector& d) const; - Vector localCoordinates(const gtsam::SimpleCamera& T2) const; - size_t dim() const; - static size_t Dim(); - - // Transformations and measurement functions - static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); - pair projectSafe(const gtsam::Point3& pw) const; - gtsam::Point2 project(const gtsam::Point3& point); - gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; - double range(const gtsam::Point3& point); - double range(const gtsam::Pose3& point); - - // enabling serialization functionality - void serialize() const; - -}; - -// Some typedefs for common camera types -// PinholeCameraCal3_S2 is the same as SimpleCamera above -typedef gtsam::PinholeCamera PinholeCameraCal3_S2; -typedef gtsam::PinholeCamera PinholeCameraCal3DS2; -typedef gtsam::PinholeCamera PinholeCameraCal3Unified; -typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; - -#include -class StereoCamera { - // Standard Constructors and Named Constructors - StereoCamera(); - StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K); - StereoCamera(const gtsam::StereoCamera& other); - - // Testable - void print(string s) const; - bool equals(const gtsam::StereoCamera& camera, double tol) const; - - // Standard Interface - gtsam::Pose3 pose() const; - double baseline() const; - gtsam::Cal3_S2Stereo calibration() const; - - // Manifold - gtsam::StereoCamera retract(const Vector& d) const; - Vector localCoordinates(const gtsam::StereoCamera& T2) const; - size_t dim() const; - static size_t Dim(); - - // Transformations and measurement functions - gtsam::StereoPoint2 project(const gtsam::Point3& point); - gtsam::Point3 backproject(const gtsam::StereoPoint2& p) const; - - // enabling serialization functionality - void serialize() const; -}; - -#include - -// Templates appear not yet supported for free functions -gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, - gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements, - double rank_tol, bool optimize); -gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, - gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements, - double rank_tol, bool optimize); - -//************************************************************************* -// Inference -//************************************************************************* - -#include -class Ordering { - // Standard Constructors and Named Constructors - Ordering(); - Ordering(const gtsam::Ordering& other); - - // Testable - void print(string s) const; - bool equals(const gtsam::Ordering& ord, double tol) const; - - // Standard interface - size_t size() const; - size_t at(size_t key) const; - void push_back(size_t key); - - // enabling serialization functionality - void serialize() const; -}; - - -//************************************************************************* -// Symbolic -//************************************************************************* - -#include -virtual class SymbolicFactor { - // Standard Constructors and Named Constructors - SymbolicFactor(const gtsam::SymbolicFactor& f); - SymbolicFactor(); - SymbolicFactor(size_t j); - SymbolicFactor(size_t j1, size_t j2); - SymbolicFactor(size_t j1, size_t j2, size_t j3); - SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4); - SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5); - SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5, size_t j6); - static gtsam::SymbolicFactor FromKeys(const gtsam::KeyVector& js); - - // From Factor - size_t size() const; - void print(string s) const; - bool equals(const gtsam::SymbolicFactor& other, double tol) const; - gtsam::KeyVector keys(); -}; - -#include -virtual class SymbolicFactorGraph { - SymbolicFactorGraph(); - SymbolicFactorGraph(const gtsam::SymbolicBayesNet& bayesNet); - SymbolicFactorGraph(const gtsam::SymbolicBayesTree& bayesTree); - SymbolicFactorGraph(const gtsam::SymbolicFactorGraph& other); - - // From FactorGraph - void push_back(gtsam::SymbolicFactor* factor); - void print(string s) const; - bool equals(const gtsam::SymbolicFactorGraph& rhs, double tol) const; - size_t size() const; - bool exists(size_t idx) const; - - // Standard interface - gtsam::KeySet keys() const; - // void push_back(gtsam::SymbolicFactor* factor); - void push_back(const gtsam::SymbolicFactorGraph& graph); - void push_back(const gtsam::SymbolicBayesNet& bayesNet); - void push_back(const gtsam::SymbolicBayesTree& bayesTree); - - //Advanced Interface - void push_factor(size_t key); - void push_factor(size_t key1, size_t key2); - void push_factor(size_t key1, size_t key2, size_t key3); - void push_factor(size_t key1, size_t key2, size_t key3, size_t key4); - - gtsam::SymbolicBayesNet* eliminateSequential(); - gtsam::SymbolicBayesNet* eliminateSequential(const gtsam::Ordering& ordering); - gtsam::SymbolicBayesTree* eliminateMultifrontal(); - gtsam::SymbolicBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering); - pair 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& variables); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables, - const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables, - const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::SymbolicFactorGraph* marginal(const gtsam::KeyVector& variables); -}; - -#include -virtual class SymbolicConditional : gtsam::SymbolicFactor { - // Standard Constructors and Named Constructors - SymbolicConditional(); - SymbolicConditional(const gtsam::SymbolicConditional& other); - SymbolicConditional(size_t key); - SymbolicConditional(size_t key, size_t parent); - SymbolicConditional(size_t key, size_t parent1, size_t parent2); - SymbolicConditional(size_t key, size_t parent1, size_t parent2, size_t parent3); - static gtsam::SymbolicConditional FromKeys(const gtsam::KeyVector& keys, size_t nrFrontals); - - // Testable - void print(string s) const; - bool equals(const gtsam::SymbolicConditional& other, double tol) const; - - // Standard interface - size_t nrFrontals() const; - size_t nrParents() const; -}; - -#include -class SymbolicBayesNet { - SymbolicBayesNet(); - SymbolicBayesNet(const gtsam::SymbolicBayesNet& other); - // Testable - void print(string s) const; - bool equals(const gtsam::SymbolicBayesNet& other, double tol) const; - - // Standard interface - size_t size() const; - void saveGraph(string s) const; - gtsam::SymbolicConditional* at(size_t idx) const; - gtsam::SymbolicConditional* front() const; - gtsam::SymbolicConditional* back() const; - void push_back(gtsam::SymbolicConditional* conditional); - void push_back(const gtsam::SymbolicBayesNet& bayesNet); -}; - -#include -class SymbolicBayesTree { - - //Constructors - SymbolicBayesTree(); - SymbolicBayesTree(const gtsam::SymbolicBayesTree& other); - - // Testable - void print(string s); - bool equals(const gtsam::SymbolicBayesTree& other, double tol) const; - - //Standard Interface - //size_t findParentClique(const gtsam::IndexVector& parents) const; - size_t size(); - void saveGraph(string s) const; - void clear(); - void deleteCachedShortcuts(); - size_t numCachedSeparatorMarginals() const; - - gtsam::SymbolicConditional* marginalFactor(size_t key) const; - gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const; - gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const; -}; - -// class SymbolicBayesTreeClique { -// BayesTreeClique(); -// BayesTreeClique(CONDITIONAL* conditional); -// // BayesTreeClique(const std::pair& result) : Base(result) {} -// -// bool equals(const This& other, double tol) const; -// void print(string s) const; -// void printTree() const; // Default indent of "" -// void printTree(string indent) const; -// size_t numCachedSeparatorMarginals() const; -// -// CONDITIONAL* conditional() const; -// bool isRoot() const; -// size_t treeSize() const; -// // const std::list& children() const { return children_; } -// // derived_ptr parent() const { return parent_.lock(); } -// -// // FIXME: 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& factorGraph); -// VariableIndex(const gtsam::GaussianFactorGraph& factorGraph); -// VariableIndex(const gtsam::NonlinearFactorGraph& factorGraph); -// VariableIndex(const gtsam::VariableIndex& other); - -// // Testable -// bool equals(const gtsam::VariableIndex& other, double tol) const; -// void print(string s) const; - -// // Standard interface -// size_t size() const; -// size_t nFactors() const; -// size_t nEntries() const; -// }; - -//************************************************************************* -// linear -//************************************************************************* - -namespace noiseModel { -#include -virtual class Base { -}; - -virtual class Gaussian : gtsam::noiseModel::Base { - static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R); - static gtsam::noiseModel::Gaussian* Covariance(Matrix R); - Matrix R() const; - bool equals(gtsam::noiseModel::Base& expected, double tol); - void print(string s) const; - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Diagonal : gtsam::noiseModel::Gaussian { - static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas); - static gtsam::noiseModel::Diagonal* Variances(Vector variances); - static gtsam::noiseModel::Diagonal* Precisions(Vector precisions); - Matrix R() const; - void print(string s) const; - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Constrained : gtsam::noiseModel::Diagonal { - static gtsam::noiseModel::Constrained* MixedSigmas(const Vector& mu, const Vector& sigmas); - static gtsam::noiseModel::Constrained* MixedSigmas(double m, const Vector& sigmas); - static gtsam::noiseModel::Constrained* MixedVariances(const Vector& mu, const Vector& variances); - static gtsam::noiseModel::Constrained* MixedVariances(const Vector& variances); - static gtsam::noiseModel::Constrained* MixedPrecisions(const Vector& mu, const Vector& precisions); - static gtsam::noiseModel::Constrained* MixedPrecisions(const Vector& precisions); - - static gtsam::noiseModel::Constrained* All(size_t dim); - static gtsam::noiseModel::Constrained* All(size_t dim, double mu); - - gtsam::noiseModel::Constrained* unit() const; - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Isotropic : gtsam::noiseModel::Diagonal { - static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma); - static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace); - static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision); - void print(string s) const; - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Unit : gtsam::noiseModel::Isotropic { - static gtsam::noiseModel::Unit* Create(size_t dim); - void print(string s) const; - - // enabling serialization functionality - void serializable() const; -}; - -namespace mEstimator { -virtual class Base { -}; - -virtual class Null: gtsam::noiseModel::mEstimator::Base { - Null(); - Null(const gtsam::noiseModel::mEstimator::Null& other); - void print(string s) const; - static gtsam::noiseModel::mEstimator::Null* Create(); - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Fair: gtsam::noiseModel::mEstimator::Base { - Fair(double c); - Fair(const gtsam::noiseModel::mEstimator::Fair& other); - void print(string s) const; - static gtsam::noiseModel::mEstimator::Fair* Create(double c); - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Huber: gtsam::noiseModel::mEstimator::Base { - Huber(double k); - Huber(const gtsam::noiseModel::mEstimator::Huber& other); - - void print(string s) const; - static gtsam::noiseModel::mEstimator::Huber* Create(double k); - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Tukey: gtsam::noiseModel::mEstimator::Base { - Tukey(double k); - Tukey(const gtsam::noiseModel::mEstimator::Tukey& other); - - void print(string s) const; - static gtsam::noiseModel::mEstimator::Tukey* Create(double k); - - // enabling serialization functionality - void serializable() const; -}; - -}///\namespace mEstimator - -virtual class Robust : gtsam::noiseModel::Base { - Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); - Robust(const gtsam::noiseModel::Robust& other); - - static gtsam::noiseModel::Robust* Create(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); - void print(string s) const; - - // enabling serialization functionality - void serializable() const; -}; - -}///\namespace noiseModel - -#include -class Sampler { - //Constructors - Sampler(gtsam::noiseModel::Diagonal* model, int seed); - Sampler(Vector sigmas, int seed); - Sampler(int seed); - Sampler(const gtsam::Sampler& other); - - - //Standard Interface - size_t dim() const; - Vector sigmas() const; - gtsam::noiseModel::Diagonal* model() const; - Vector sample(); - Vector sampleNewModel(gtsam::noiseModel::Diagonal* model); -}; - -#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) 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; -}; - -#include -virtual class GaussianFactor { - gtsam::KeyVector keys() const; - void print(string s) 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); - JacobianFactor(const gtsam::JacobianFactor& other); - - //Testable - void print(string s) const; - void printKeys(string s) const; - bool equals(const gtsam::GaussianFactor& lf, double tol) const; - size_t size() const; - Vector unweighted_error(const gtsam::VectorValues& c) const; - Vector error_vector(const gtsam::VectorValues& c) const; - double error(const gtsam::VectorValues& c) const; - - //Standard Interface - Matrix py_getA() const; - Vector py_getb() const; - size_t rows() const; - size_t cols() const; - bool isConstrained() const; - pair jacobianUnweighted() const; - Matrix augmentedJacobianUnweighted() const; - - void transposeMultiplyAdd(double alpha, const Vector& e, gtsam::VectorValues& x) const; - gtsam::JacobianFactor whiten() const; - - pair eliminate(const gtsam::Ordering& keys) const; - - void setModel(bool anyConstrained, const Vector& sigmas); - - gtsam::noiseModel::Diagonal* get_model() const; - - // enabling serialization functionality - void serialize() 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); - HessianFactor(const gtsam::HessianFactor& other); - - //Testable - size_t size() const; - void print(string s) 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; -}; - -#include -class GaussianFactorGraph { - GaussianFactorGraph(); - GaussianFactorGraph(const gtsam::GaussianBayesNet& bayesNet); - GaussianFactorGraph(const gtsam::GaussianBayesTree& bayesTree); - GaussianFactorGraph(const gtsam::GaussianFactorGraph& other); - - // From FactorGraph - void print(string s) const; - bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const; - size_t size() const; - gtsam::GaussianFactor* at(size_t idx) const; - gtsam::KeySet keys() const; - bool exists(size_t idx) const; - - // Building the graph - void push_back(const gtsam::GaussianFactor* factor); - void push_back(const gtsam::GaussianConditional* factor); - void push_back(const gtsam::GaussianFactorGraph& graph); - void push_back(const gtsam::GaussianBayesNet& bayesNet); - void push_back(const gtsam::GaussianBayesTree& bayesTree); - void add(const gtsam::GaussianFactor& factor); - void add(Vector b); - void add(size_t key1, Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); - void add(size_t key1, Matrix A1, size_t key2, Matrix A2, Vector b, - const gtsam::noiseModel::Diagonal* model); - void add(size_t key1, Matrix A1, size_t key2, Matrix A2, size_t key3, Matrix A3, - Vector b, const gtsam::noiseModel::Diagonal* model); - - // error and probability - double error(const gtsam::VectorValues& c) const; - double probPrime(const gtsam::VectorValues& c) const; - - gtsam::GaussianFactorGraph clone() const; - gtsam::GaussianFactorGraph negate() const; - - // Optimizing and linear algebra - gtsam::VectorValues optimize() const; - gtsam::VectorValues optimize(const gtsam::Ordering& ordering) const; - gtsam::VectorValues optimizeGradientSearch() const; - gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; - gtsam::VectorValues gradientAtZero() const; - - // Elimination and marginals - gtsam::GaussianBayesNet* eliminateSequential(); - gtsam::GaussianBayesNet* eliminateSequential(const gtsam::Ordering& ordering); - gtsam::GaussianBayesTree* eliminateMultifrontal(); - gtsam::GaussianBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering); - pair 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& variables); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables, - const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables, - const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::GaussianFactorGraph* marginal(const gtsam::KeyVector& variables); - - // Conversion to matrices - Matrix sparseJacobian_() const; - Matrix augmentedJacobian() const; - Matrix augmentedJacobian(const gtsam::Ordering& ordering) const; - pair 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; -}; - -#include -virtual class GaussianConditional : gtsam::GaussianFactor { - //Constructors - GaussianConditional(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - const gtsam::noiseModel::Diagonal* sigmas); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - size_t name2, Matrix T, const gtsam::noiseModel::Diagonal* sigmas); - - //Constructors with no noise model - GaussianConditional(size_t key, Vector d, Matrix R); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - size_t name2, Matrix T); - GaussianConditional(const gtsam::GaussianConditional& other); - - //Standard Interface - void print(string s) const; - bool equals(const gtsam::GaussianConditional &cg, double tol) const; - - //Advanced Interface - gtsam::VectorValues solve(const gtsam::VectorValues& parents) const; - gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents, const gtsam::VectorValues& rhs) const; - void solveTransposeInPlace(gtsam::VectorValues& gy) const; - void scaleFrontalsBySigma(gtsam::VectorValues& gy) const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -virtual class GaussianDensity : gtsam::GaussianConditional { - //Constructors - GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); - GaussianDensity(const gtsam::GaussianDensity& other); - - //Standard Interface - void print(string s) 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); - GaussianBayesNet(const gtsam::GaussianBayesNet& other); - - // Testable - void print(string s) const; - bool equals(const gtsam::GaussianBayesNet& other, double tol) const; - size_t size() const; - - // FactorGraph derived interface - // size_t size() const; - gtsam::GaussianConditional* at(size_t idx) const; - gtsam::KeySet keys() const; - bool exists(size_t idx) const; - - gtsam::GaussianConditional* front() const; - gtsam::GaussianConditional* back() const; - void push_back(gtsam::GaussianConditional* conditional); - void push_back(const gtsam::GaussianBayesNet& bayesNet); - - gtsam::VectorValues optimize() const; - gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const; - gtsam::VectorValues optimizeGradientSearch() const; - gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; - gtsam::VectorValues gradientAtZero() const; - double error(const gtsam::VectorValues& x) const; - double determinant() const; - double logDeterminant() const; - gtsam::VectorValues backSubstitute(const gtsam::VectorValues& gx) const; - gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues& gx) const; -}; - -#include -virtual class GaussianBayesTree { - // Standard Constructors and Named Constructors - GaussianBayesTree(); - GaussianBayesTree(const gtsam::GaussianBayesTree& other); - bool equals(const gtsam::GaussianBayesTree& other, double tol) const; - void print(string s); - size_t size() const; - bool empty() const; - 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); - Errors(const gtsam::Errors& other); - - //Testable - void print(string s); - bool equals(const gtsam::Errors& expected, double tol) const; -}; - -#include -class GaussianISAM { - //Constructor - GaussianISAM(); - GaussianISAM(const gtsam::GaussianISAM& other); - - //Standard Interface - void update(const gtsam::GaussianFactorGraph& newFactors); - void saveGraph(string s) const; - void clear(); -}; - -#include -virtual class IterativeOptimizationParameters { - IterativeOptimizationParameters(const gtsam::IterativeOptimizationParameters& other); - string getVerbosity() const; - void setVerbosity(string s) ; - void print() const; -}; - -//virtual class IterativeSolver { -// IterativeSolver(); -// gtsam::VectorValues optimize (); -//}; - -#include -virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParameters { - ConjugateGradientParameters(); - ConjugateGradientParameters(const gtsam::ConjugateGradientParameters& other); - - int getMinIterations() const ; - int getMaxIterations() const ; - int getReset() const; - double getEpsilon_rel() const; - double getEpsilon_abs() const; - - void setMinIterations(int value); - void setMaxIterations(int value); - void setReset(int value); - void setEpsilon_rel(double value); - void setEpsilon_abs(double value); - void print() const; -}; - -#include -virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters { - SubgraphSolverParameters(); - SubgraphSolverParameters(const gtsam::SubgraphSolverParameters& other); - void print() const; -}; - -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); - SubgraphSolver(const gtsam::SubgraphSolver& other); - - gtsam::VectorValues optimize() const; -}; - -#include -class KalmanFilter { - KalmanFilter(size_t n); - KalmanFilter(const gtsam::KalmanFilter& other); - - // 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 -size_t symbol(char chr, size_t index); -char symbolChr(size_t key); -size_t symbolIndex(size_t key); - -// Default keyformatter -void PrintKeyList (const gtsam::KeyList& keys); -void PrintKeyList (const gtsam::KeyList& keys, string s); -void PrintKeyVector(const gtsam::KeyVector& keys); -void PrintKeyVector(const gtsam::KeyVector& keys, string s); -void PrintKeySet (const gtsam::KeySet& keys); -void PrintKeySet (const gtsam::KeySet& keys, string s); - -#include -class LabeledSymbol { - 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 Values { - Values(); - Values(const gtsam::Values& other); - - size_t size() const; - bool empty() const; - void clear(); - size_t dim() const; - - void print(string s) const; - bool equals(const gtsam::Values& other, double tol) const; - - void insert(const gtsam::Values& values); - void update(const gtsam::Values& values); - void erase(size_t j); - void swap(gtsam::Values& values); - - bool exists(size_t j) const; - gtsam::KeyVector keys() const; - - gtsam::VectorValues zeroVectors() const; - - gtsam::Values retract(const gtsam::VectorValues& delta) const; - gtsam::VectorValues localCoordinates(const gtsam::Values& cp) const; - - // enabling serialization functionality - void serialize() const; - - // New in 4.0, we have to specialize every insert/update/at to generate wrappers - // Instead of the old: - // void insert(size_t j, const gtsam::Value& value); - // void update(size_t j, const gtsam::Value& val); - // gtsam::Value at(size_t j) const; - - template - void insert(size_t j, const T& t); - - template - void update(size_t j, const T& t); - - template - T at(size_t j); - - /// version for double - void insertDouble(size_t j, double c); - double atDouble(size_t j) const; -}; - -#include -virtual class NonlinearFactor { - // Factor base class - size_t size() const; - // gtsam::KeyVector keys() const; - void print(string s) const; - void printKeys(string s) const; - // NonlinearFactor - 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; //FIXME: Conversion from KeyVector to std::vector does not happen -}; - -#include -class NonlinearFactorGraph { - NonlinearFactorGraph(); - NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph); - - // FactorGraph - void print(string s) const; - bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const; - size_t size() const; - bool empty() const; - void remove(size_t i); - size_t nrFactors() const; - gtsam::NonlinearFactor* at(size_t idx) const; - void push_back(const gtsam::NonlinearFactorGraph& factors); - void push_back(gtsam::NonlinearFactor* factor); - void add(gtsam::NonlinearFactor* factor); - bool exists(size_t idx) const; - // gtsam::KeySet keys() const; - - // NonlinearFactorGraph - double error(const gtsam::Values& values) const; - double probPrime(const gtsam::Values& values) const; - gtsam::Ordering orderingCOLAMD() const; - // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map& constraints) const; - gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values) const; - gtsam::NonlinearFactorGraph clone() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -virtual class NoiseModelFactor: gtsam::NonlinearFactor { - void equals(const gtsam::NoiseModelFactor& other, double tol) const; - gtsam::noiseModel::Base* get_noiseModel() const; // deprecated by below - gtsam::noiseModel::Base* noiseModel() const; - Vector unwhitenedError(const gtsam::Values& x) const; - Vector whitenedError(const gtsam::Values& x) const; -}; - -// // Actually a FastList -// #include -// class KeyList { -// KeyList(); -// KeyList(const gtsam::KeyList& other); - -// // Note: no print function - -// // common STL methods -// size_t size() const; -// bool empty() const; -// void clear(); - -// // structure specific methods -// size_t front() const; -// size_t back() const; -// void push_back(size_t key); -// void push_front(size_t key); -// void pop_back(); -// void pop_front(); -// void sort(); -// void remove(size_t key); - -// void serialize() const; -// }; - -// // Actually a FastSet -// class KeySet { -// KeySet(); -// KeySet(const gtsam::KeySet& other); -// KeySet(const gtsam::KeyVector& other); -// KeySet(const gtsam::KeyList& other); - -// // Testable -// void print(string s) const; -// bool equals(const gtsam::KeySet& other) const; - -// // common STL methods -// size_t size() const; -// bool empty() const; -// void clear(); - -// // structure specific methods -// void insert(size_t key); -// void merge(gtsam::KeySet& other); -// bool erase(size_t key); // returns true if value was removed -// bool count(size_t key) const; // returns true if value exists - -// void serialize() const; -// }; - -// // Actually a vector -// class KeyVector { -// KeyVector(); -// KeyVector(const gtsam::KeyVector& other); - -// // Note: no print function - -// // common STL methods -// size_t size() const; -// bool empty() const; -// void clear(); - -// // structure specific methods -// size_t at(size_t i) const; -// size_t front() const; -// size_t back() const; -// void push_back(size_t key) const; - -// void serialize() const; -// }; - -// // Actually a FastMap -// class KeyGroupMap { -// KeyGroupMap(); - -// // Note: no print function - -// // common STL methods -// size_t size() const; -// bool empty() const; -// void clear(); - -// // structure specific methods -// size_t at(size_t key) const; -// int erase(size_t key); -// bool insert2(size_t key, int val); -// }; - -typedef gtsam::FastMap KeyGroupMap; - -#include -class JointMarginal { - JointMarginal(const JointMarginal& j); - Matrix at(size_t iVariable, size_t jVariable) const; - Matrix fullMatrix() const; - void print(string s) const; - void print() const; -}; - -class Marginals { - Marginals(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& solution); - Marginals(const gtsam::Marginals& other); - - void print(string s) const; - Matrix marginalCovariance(size_t variable) const; - Matrix marginalInformation(size_t variable) const; - gtsam::JointMarginal jointMarginalCovariance(const gtsam::KeyVector& variables) const; - gtsam::JointMarginal jointMarginalInformation(const gtsam::KeyVector& variables) const; -}; - - - -#include -virtual class LinearContainerFactor : gtsam::NonlinearFactor { - LinearContainerFactor(const gtsam::LinearContainerFactor& other); - - 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(); - NonlinearOptimizerParams(const gtsam::NonlinearOptimizerParams& other); - void print(string s) const; - - int getMaxIterations() const; - double getRelativeErrorTol() const; - double getAbsoluteErrorTol() const; - double getErrorTol() const; - string getVerbosity() const; - - void setMaxIterations(int value); - void setRelativeErrorTol(double value); - void setAbsoluteErrorTol(double value); - void setErrorTol(double value); - void setVerbosity(string s); - - string getLinearSolverType() const; - - void setLinearSolverType(string solver); - void setOrdering(const gtsam::Ordering& ordering); - void setIterativeParams(gtsam::IterativeOptimizationParameters* params); - - bool isMultifrontal() const; - bool isSequential() const; - bool isCholmod() const; - bool isIterative() const; -}; - -bool checkConvergence(double relativeErrorTreshold, - double absoluteErrorTreshold, double errorThreshold, - double currentError, double newError); - -#include -virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams { - GaussNewtonParams(); - GaussNewtonParams(const gtsam::GaussNewtonParams& other); -}; - -#include -virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams { - LevenbergMarquardtParams(); - LevenbergMarquardtParams(const gtsam::LevenbergMarquardtParams& other); - - double getlambdaInitial() const; - double getlambdaFactor() const; - double getlambdaUpperBound() const; - string getVerbosityLM() const; - - void setlambdaInitial(double value); - void setlambdaFactor(double value); - void setlambdaUpperBound(double value); - void setVerbosityLM(string s); -}; - -#include -virtual class DoglegParams : gtsam::NonlinearOptimizerParams { - DoglegParams(); - DoglegParams(const gtsam::DoglegParams& other); - - 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; - void 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 str) const; -}; - -#include -class ISAM2GaussNewtonParams { - ISAM2GaussNewtonParams(); - ISAM2GaussNewtonParams(const gtsam::ISAM2GaussNewtonParams& other); - - void print(string str) const; - - /** Getters and Setters for all properties */ - double getWildfireThreshold() const; - void setWildfireThreshold(double wildfireThreshold); -}; - -class ISAM2DoglegParams { - ISAM2DoglegParams(); - ISAM2DoglegParams(const gtsam::ISAM2DoglegParams& other); - - void print(string str) const; - - /** Getters and Setters for all properties */ - double getWildfireThreshold() const; - void setWildfireThreshold(double wildfireThreshold); - double getInitialDelta() const; - void setInitialDelta(double initialDelta); - string getAdaptationMode() const; - void setAdaptationMode(string adaptationMode); - bool isVerbose() const; - void setVerbose(bool verbose); -}; - -class ISAM2ThresholdMapValue { - ISAM2ThresholdMapValue(char c, Vector thresholds); - ISAM2ThresholdMapValue(const gtsam::ISAM2ThresholdMapValue& other); -}; - -class ISAM2ThresholdMap { - ISAM2ThresholdMap(); - ISAM2ThresholdMap(const gtsam::ISAM2ThresholdMap& other); - - // Note: no print function - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - void insert(const gtsam::ISAM2ThresholdMapValue& value) const; -}; - -class ISAM2Params { - ISAM2Params(); - ISAM2Params(const gtsam::ISAM2Params& other); - - void print(string str) const; - - /** Getters and Setters for all properties */ - void setOptimizationParams(const gtsam::ISAM2GaussNewtonParams& params); - void setOptimizationParams(const gtsam::ISAM2DoglegParams& params); - void setRelinearizeThreshold(double relinearizeThreshold); - void setRelinearizeThreshold(const gtsam::ISAM2ThresholdMap& relinearizeThreshold); - int getRelinearizeSkip() const; - void setRelinearizeSkip(int relinearizeSkip); - bool isEnableRelinearization() const; - void setEnableRelinearization(bool enableRelinearization); - bool isEvaluateNonlinearError() const; - void setEvaluateNonlinearError(bool evaluateNonlinearError); - string getFactorization() const; - void setFactorization(string factorization); - bool isCacheLinearizedFactors() const; - void setCacheLinearizedFactors(bool cacheLinearizedFactors); - bool isEnableDetailedResults() const; - void setEnableDetailedResults(bool enableDetailedResults); - bool isEnablePartialRelinearizationCheck() const; - void setEnablePartialRelinearizationCheck(bool enablePartialRelinearizationCheck); -}; - -class ISAM2Clique { - - //Constructors - ISAM2Clique(); - ISAM2Clique(const gtsam::ISAM2Clique& other); - - //Standard Interface - Vector gradientContribution() const; - void print(string s); -}; - -class ISAM2Result { - ISAM2Result(); - ISAM2Result(const gtsam::ISAM2Result& other); - - void print(string str) const; - - /** Getters and Setters for all properties */ - size_t getVariablesRelinearized() const; - size_t getVariablesReeliminated() const; - size_t getCliques() const; -}; - -class FactorIndices {}; - -class ISAM2 { - ISAM2(); - ISAM2(const gtsam::ISAM2Params& params); - ISAM2(const gtsam::ISAM2& other); - - bool equals(const gtsam::ISAM2& other, double tol) const; - void print(string s) const; - void printStats() const; - void saveGraph(string s) const; - - gtsam::ISAM2Result update(); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, const gtsam::KeyGroupMap& constrainedKeys); - // TODO: wrap the full version of update - //void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap& constrainedKeys); - //void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap& constrainedKeys, bool force_relinearize); - - gtsam::Values getLinearizationPoint() const; - gtsam::Values calculateEstimate() const; - template - VALUE calculateEstimate(size_t key) const; - gtsam::Values calculateBestEstimate() const; - Matrix marginalCovariance(size_t key) const; - gtsam::VectorValues getDelta() const; - gtsam::NonlinearFactorGraph getFactorsUnsafe() const; - // gtsam::VariableIndex getVariableIndex() const; - gtsam::ISAM2Params params() const; -}; - -#include -class NonlinearISAM { - NonlinearISAM(); - NonlinearISAM(int reorderInterval); - NonlinearISAM(const gtsam::NonlinearISAM& other); - - void print(string s) const; - void printStats() const; - void saveGraph(string s) const; - gtsam::Values estimate() const; - Matrix marginalCovariance(size_t key) const; - int reorderInterval() const; - int reorderCounter() const; - void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& initialValues); - void reorder_relinearize(); - - // These might be expensive as instead of a reference the wrapper will make a copy - gtsam::GaussianISAM bayesTree() const; - gtsam::Values getLinearizationPoint() const; - gtsam::NonlinearFactorGraph getFactorsUnsafe() const; -}; - -//************************************************************************* -// Nonlinear factor types -//************************************************************************* -#include -#include -#include - -#include -template -virtual class PriorFactor : gtsam::NoiseModelFactor { - PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); - PriorFactor(const This& other); - T prior() const; - - // enabling serialization functionality - void serialize() const; -}; - - -#include -template -virtual class BetweenFactor : gtsam::NoiseModelFactor { - BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); - BetweenFactor(const This& other); - T measured() const; - - // enabling serialization functionality - void serialize() 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); - NonlinearEquality(const This& other); - - // 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); - RangeFactor(const This& other); -}; - -typedef gtsam::RangeFactor RangeFactorPosePoint2; -typedef gtsam::RangeFactor RangeFactorPosePoint3; -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 BearingFactor : gtsam::NoiseModelFactor { - BearingFactor(size_t key1, size_t key2, const BEARING& measured, const gtsam::noiseModel::Base* noiseModel); - BearingFactor(const This& other); - - // enabling serialization functionality - void serialize() const; -}; - -typedef gtsam::BearingFactor BearingFactor2D; - -#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); - BearingRangeFactor(const This& other); - - // enabling serialization functionality - void serialize() const; -}; - -typedef gtsam::BearingRangeFactor BearingRangeFactor2D; - - -#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); - GenericProjectionFactor(const This& other); - - 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); - GeneralSFMFactor(const This& other); - gtsam::Point2 measured() const; -}; -typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; -typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; - -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); - GeneralSFMFactor2(const This& other); - gtsam::Point2 measured() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -class SmartProjectionParams { - SmartProjectionParams(); - SmartProjectionParams(const gtsam::SmartProjectionParams& other); - // TODO(frank): make these work: - // void setLinearizationMode(LinearizationMode linMode); - // void setDegeneracyMode(DegeneracyMode degMode); - void setRankTolerance(double rankTol); - void setEnableEPI(bool enableEPI); - void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold); - void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold); -}; - -#include -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::Pose3& body_P_sensor, - const gtsam::SmartProjectionParams& params); - SmartProjectionPoseFactor(const This& other); - - 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); - GenericStereoFactor(const This& other); - 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); - PoseTranslationPrior(const This& other); -}; - -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); - PoseRotationPrior(const This& other); -}; - -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); - EssentialMatrixFactor(const gtsam::EssentialMatrixFactor& other); -}; - -#include -pair load2D(string filename, - gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart); -pair load2D(string filename, - gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise); -pair load2D(string filename, - gtsam::noiseModel::Diagonal* model, int maxID); -pair load2D(string filename, - gtsam::noiseModel::Diagonal* model); -pair load2D(string filename); -pair load2D_robust(string filename, - gtsam::noiseModel::Base* model); -void save2D(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& config, gtsam::noiseModel::Diagonal* model, - string filename); - -pair readG2o(string filename); -void writeG2o(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& estimate, string filename); - -//************************************************************************* -// Navigation -//************************************************************************* -namespace imuBias { -#include - -class ConstantBias { - // Constructors - ConstantBias(); - ConstantBias(Vector biasAcc, Vector biasGyro); - ConstantBias(const gtsam::imuBias::ConstantBias& other); - - // Testable - void print(string s) const; - bool equals(const gtsam::imuBias::ConstantBias& expected, double tol) const; - - // Group - static gtsam::imuBias::ConstantBias identity(); - gtsam::imuBias::ConstantBias inverse() const; - gtsam::imuBias::ConstantBias compose(const gtsam::imuBias::ConstantBias& b) const; - gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias& b) const; - - // Manifold - gtsam::imuBias::ConstantBias retract(Vector v) const; - Vector localCoordinates(const gtsam::imuBias::ConstantBias& b) const; - - // Lie Group - static gtsam::imuBias::ConstantBias Expmap(Vector v); - static Vector Logmap(const gtsam::imuBias::ConstantBias& b); - - // Standard Interface - Vector vector() const; - Vector accelerometer() const; - Vector gyroscope() const; - Vector correctAccelerometer(Vector measurement) const; - Vector correctGyroscope(Vector measurement) const; -}; - -}///\namespace imuBias - -#include -class NavState { - // Constructors - NavState(); - NavState(const gtsam::Rot3& R, const gtsam::Point3& t, Vector v); - NavState(const gtsam::Pose3& pose, Vector v); - NavState(const gtsam::NavState& other); - - // Testable - void print(string s) const; - bool equals(const gtsam::NavState& expected, double tol) const; - - // Access - gtsam::Rot3 attitude() const; - gtsam::Point3 position() const; - Vector velocity() const; - gtsam::Pose3 pose() const; -}; - -#include -virtual class PreintegratedRotationParams { - PreintegratedRotationParams(); - PreintegratedRotationParams(const gtsam::PreintegratedRotationParams& other); - void setGyroscopeCovariance(Matrix cov); - void setOmegaCoriolis(Vector omega); - void setBodyPSensor(const gtsam::Pose3& pose); - - Matrix getGyroscopeCovariance() const; - - // TODO(frank): allow optional - // boost::optional getOmegaCoriolis() const; - // boost::optional getBodyPSensor() const; -}; - -#include -virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { - PreintegrationParams(Vector n_gravity); - PreintegrationParams(const gtsam::PreintegrationParams& other); - 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); - PreintegratedImuMeasurements(const gtsam::PreintegratedImuMeasurements& other); - - // Testable - void print(string s) const; - bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol); - - // Standard Interface - void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, - double deltaT); - void resetIntegration(); - Matrix preintMeasCov() const; - double deltaTij() const; - gtsam::Rot3 deltaRij() const; - Vector deltaPij() const; - Vector deltaVij() const; - Vector biasHatVector() const; - gtsam::NavState predict(const gtsam::NavState& state_i, - const gtsam::imuBias::ConstantBias& bias) const; -}; - -virtual class ImuFactor: gtsam::NonlinearFactor { - ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, - size_t bias, - const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements); - ImuFactor(const gtsam::ImuFactor& other); - - // 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 -class PreintegratedCombinedMeasurements { - PreintegratedCombinedMeasurements(const gtsam::PreintegratedCombinedMeasurements& other); - // Testable - void print(string s) const; - bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, - double tol); - - // Standard Interface - void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, - double deltaT); - void resetIntegration(); - Matrix preintMeasCov() const; - double deltaTij() const; - gtsam::Rot3 deltaRij() const; - Vector deltaPij() const; - Vector deltaVij() const; - Vector biasHatVector() const; - gtsam::NavState predict(const gtsam::NavState& state_i, - const gtsam::imuBias::ConstantBias& bias) const; -}; - -virtual class CombinedImuFactor: gtsam::NonlinearFactor { - CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, - size_t bias_i, size_t bias_j, - const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements); - CombinedImuFactor(const gtsam::CombinedImuFactor& other); - - // 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) 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); - AHRSFactor(const gtsam::AHRSFactor& rhs); - - // 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(); - Rot3AttitudeFactor(const gtsam::Rot3AttitudeFactor& rhs); - - void print(string s) const; - bool equals(const gtsam::NonlinearFactor& expected, double tol) const; - gtsam::Unit3 nZ() const; - gtsam::Unit3 bRef() const; -}; - -virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor{ - Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model, - const gtsam::Unit3& bRef); - Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model); - Pose3AttitudeFactor(); - Pose3AttitudeFactor(const gtsam::Pose3AttitudeFactor& rhs); - void print(string s) const; - bool equals(const gtsam::NonlinearFactor& expected, double tol) const; - gtsam::Unit3 nZ() const; - gtsam::Unit3 bRef() const; -}; - -//************************************************************************* -// Utilities -//************************************************************************* - -namespace utilities { - - #include - gtsam::KeyList createKeyList(Vector I); - gtsam::KeyList createKeyList(string s, Vector I); - gtsam::KeyVector createKeyVector(Vector I); - gtsam::KeyVector createKeyVector(string s, Vector I); - gtsam::KeySet createKeySet(Vector I); - gtsam::KeySet createKeySet(string s, Vector I); - Matrix extractPoint2(const gtsam::Values& values); - Matrix extractPoint3(const gtsam::Values& values); - Matrix extractPose2(const gtsam::Values& values); - gtsam::Values allPose3s(gtsam::Values& values); - Matrix extractPose3(const gtsam::Values& values); - void perturbPoint2(gtsam::Values& values, double sigma, int seed); - void perturbPose2 (gtsam::Values& values, double sigmaT, double sigmaR, int seed); - void perturbPoint3(gtsam::Values& values, double sigma, int seed); - void insertBackprojections(gtsam::Values& values, const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth); - void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K); - void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor); - Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values); - gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base); - gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base, const gtsam::KeyVector& keys); - -} //\namespace utilities - -} diff --git a/wrap/tests/testWrapCython.cpp b/wrap/tests/testWrapCython.cpp deleted file mode 100644 index 37141c4b0..000000000 --- a/wrap/tests/testWrapCython.cpp +++ /dev/null @@ -1,59 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testWrap.cpp - * @brief Unit test for wrap.c - * @author Frank Dellaert - **/ - -#include -#include - -#include - -#include -#include - -#include -#include -#include -#include - -using namespace std; -using namespace boost::assign; -using namespace wrap; -namespace fs = boost::filesystem; -static bool enable_verbose = true; -#ifdef TOPSRCDIR -static string topdir = TOPSRCDIR; -#else -static string topdir = "TOPSRCDIR_NOT_CONFIGURED"; // If TOPSRCDIR is not defined, we error -#endif - -/* ************************************************************************* */ -TEST( wrap, cython_code_geometry ) { - // Parse into class object - string header_path = topdir + "/wrap/tests"; - Module module(header_path,"cythontest",enable_verbose); - string path = topdir + "/wrap"; - - // clean out previous generated code - fs::remove_all("actual-cython"); - - // emit MATLAB code - // make_geometry will not compile, use make testwrap to generate real make - module.cython_wrapper("actual-cython"); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */