From 6fbb8cdff8b8b4420ad1a3277e617a7a42ab1275 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 26 Jan 2016 11:42:43 -0800 Subject: [PATCH 1/6] Added gtsampy.h header to support development of py_wrap branch: the differences between gtsam.h and gtsampy.h highlight the progress of python wrapping. --- gtsampy.h | 2669 +++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 2669 insertions(+) create mode 100644 gtsampy.h diff --git a/gtsampy.h b/gtsampy.h new file mode 100644 index 000000000..ca014ad5d --- /dev/null +++ b/gtsampy.h @@ -0,0 +1,2669 @@ +/** + + * GTSAM Wrap Module Definition + * + * These are the current classes available through the matlab toolbox interface, + * add more functions/classes as they are available. + * + * Requirements: + * Classes must start with an uppercase letter + * - Can wrap a typedef + * Only one Method/Constructor per line, though methods/constructors can extend across multiple lines + * Methods can return + * - Eigen types: Matrix, Vector + * - C/C++ basic types: string, bool, size_t, int, double, char, unsigned char + * - void + * - Any class with which be copied with boost::make_shared() + * - boost::shared_ptr of any object type + * Constructors + * - Overloads are supported + * - A class with no constructors can be returned from other functions but not allocated directly in MATLAB + * Methods + * - Constness has no effect + * - Specify by-value (not reference) return types, even if C++ method returns reference + * - Must start with a letter (upper or lowercase) + * - Overloads are supported + * Static methods + * - Must start with a letter (upper or lowercase) and use the "static" keyword + * - The first letter will be made uppercase in the generated MATLAB interface + * - Overloads are supported + * Arguments to functions any of + * - Eigen types: Matrix, Vector + * - Eigen types and classes as an optionally const reference + * - C/C++ basic types: string, bool, size_t, size_t, double, char, unsigned char + * - Any class with which be copied with boost::make_shared() (except Eigen) + * - boost::shared_ptr of any object type (except Eigen) + * Comments can use either C++ or C style, with multiple lines + * Namespace definitions + * - Names of namespaces must start with a lowercase letter + * - start a namespace with "namespace {" + * - end a namespace with exactly "}" + * - Namespaces can be nested + * Namespace usage + * - Namespaces can be specified for classes in arguments and return values + * - In each case, the namespace must be fully specified, e.g., "namespace1::namespace2::ClassName" + * Includes in C++ wrappers + * - All includes will be collected and added in a single file + * - All namespaces must have angle brackets: + * - No default includes will be added + * Global/Namespace functions + * - Functions specified outside of a class are global + * - Can be overloaded with different arguments + * - Can have multiple functions of the same name in different namespaces + * Using classes defined in other modules + * - If you are using a class 'OtherClass' not wrapped in this definition file, add "class OtherClass;" to avoid a dependency error + * Virtual inheritance + * - Specify fully-qualified base classes, i.e. "virtual class Derived : ns::Base {" where "ns" is the namespace + * - Mark with 'virtual' keyword, e.g. "virtual class Base {", and also "virtual class Derived : ns::Base {" + * - Forward declarations must also be marked virtual, e.g. "virtual class ns::Base;" and + * also "virtual class ns::Derived;" + * - Pure virtual (abstract) classes should list no constructors in this interface file + * - Virtual classes must have a clone() function in C++ (though it does not have to be included + * in the MATLAB interface). clone() will be called whenever an object copy is needed, instead + * of using the copy constructor (which is used for non-virtual objects). + * - Signature of clone function - will be called virtually, so must appear at least at the top of the inheritance tree + * virtual boost::shared_ptr clone() const; + * Class Templates + * - Basic templates are supported either with an explicit list of types to instantiate, + * e.g. template class Class1 { ... }; + * or with typedefs, e.g. + * template class Class2 { ... }; + * typedef Class2 MyInstantiatedClass; + * - In the class definition, appearances of the template argument(s) will be replaced with their + * instantiated types, e.g. 'void setValue(const T& value);'. + * - To refer to the instantiation of the template class itself, use 'This', i.e. 'static This Create();' + * - To create new instantiations in other modules, you must copy-and-paste the whole class definition + * into the new module, but use only your new instantiation types. + * - When forward-declaring template instantiations, use the generated/typedefed name, e.g. + * class gtsam::Class1Pose2; + * class gtsam::MyInstantiatedClass; + * Boost.serialization within Matlab: + * - you need to mark classes as being serializable in the markup file (see this file for an example). + * - There are two options currently, depending on the class. To "mark" a class as serializable, + * add a function with a particular signature so that wrap will catch it. + * - Add "void serialize()" to a class to create serialization functions for a class. + * Adding this flag subsumes the serializable() flag below. Requirements: + * - A default constructor must be publicly accessible + * - Must not be an abstract base class + * - The class must have an actual boost.serialization serialize() function. + * - Add "void serializable()" to a class if you only want the class to be serialized as a + * part of a container (such as noisemodel). This version does not require a publicly + * accessible default constructor. + */ + +/** + * Status: + * - TODO: default values for arguments + * - WORKAROUND: make multiple versions of the same function for different configurations of default arguments + * - TODO: Handle gtsam::Rot3M conversions to quaternions + * - TODO: Parse return of const ref arguments + * - TODO: Parse std::string variants and convert directly to special string + * - TODO: Add enum support + * - TODO: Add generalized serialization support via boost.serialization with hooks to matlab save/load + */ + +namespace std { + #include + template + class vector + { + //Do we need these? + //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 + T* at(size_t n); + T* front(); + T* back(); + + //Modifiers + void assign(size_t n, const T& u); + void push_back(const T& x); + void pop_back();*/ + }; + //typedef std::vector + + #include + template + class list + { + + + }; + +} + +namespace gtsam { + +//************************************************************************* +// base +//************************************************************************* + +/** gtsam namespace functions */ +bool linear_independent(Matrix A, Matrix B, double tol); + +virtual class Value { + // No constructors because this is an abstract class + + // Testable + void print(string s) const; + + // Manifold + size_t dim() const; +}; + +#include +class LieScalar { + // Standard constructors + LieScalar(); + LieScalar(double d); + + // Standard interface + double value() const; + + // Testable + void print(string s) const; + bool equals(const gtsam::LieScalar& expected, double tol) const; + + // Group + static gtsam::LieScalar identity(); + gtsam::LieScalar inverse() const; + gtsam::LieScalar compose(const gtsam::LieScalar& p) const; + gtsam::LieScalar between(const gtsam::LieScalar& l2) const; + + // Manifold + size_t dim() const; + gtsam::LieScalar retract(Vector v) const; + Vector localCoordinates(const gtsam::LieScalar& t2) const; + + // Lie group + static gtsam::LieScalar Expmap(Vector v); + static Vector Logmap(const gtsam::LieScalar& p); +}; + +#include +class LieVector { + // Standard constructors + LieVector(); + LieVector(Vector v); + + // Standard interface + Vector vector() const; + + // Testable + void print(string s) const; + bool equals(const gtsam::LieVector& expected, double tol) const; + + // Group + static gtsam::LieVector identity(); + gtsam::LieVector inverse() const; + gtsam::LieVector compose(const gtsam::LieVector& p) const; + gtsam::LieVector between(const gtsam::LieVector& l2) const; + + // Manifold + size_t dim() const; + gtsam::LieVector retract(Vector v) const; + Vector localCoordinates(const gtsam::LieVector& t2) const; + + // Lie group + static gtsam::LieVector Expmap(Vector v); + static Vector Logmap(const gtsam::LieVector& p); + + // enabling serialization functionality + void serialize() const; +}; + +#include +class LieMatrix { + // Standard constructors + LieMatrix(); + LieMatrix(Matrix v); + + // Standard interface + Matrix matrix() const; + + // Testable + void print(string s) const; + bool equals(const gtsam::LieMatrix& expected, double tol) const; + + // Group + static gtsam::LieMatrix identity(); + gtsam::LieMatrix inverse() const; + gtsam::LieMatrix compose(const gtsam::LieMatrix& p) const; + gtsam::LieMatrix between(const gtsam::LieMatrix& l2) const; + + // Manifold + size_t dim() const; + gtsam::LieMatrix retract(Vector v) const; + Vector localCoordinates(const gtsam::LieMatrix & t2) const; + + // Lie group + static gtsam::LieMatrix Expmap(Vector v); + static Vector Logmap(const gtsam::LieMatrix& p); + + // enabling serialization functionality + void serialize() const; +}; + +//************************************************************************* +// geometry +//************************************************************************* + +class Point2 { + // Standard Constructors + Point2(); + Point2(double x, double y); + Point2(Vector v); + + // Testable + void print(string s) const; + bool equals(const gtsam::Point2& pose, double tol) const; + + // Group + static gtsam::Point2 identity(); + gtsam::Point2 inverse() const; + gtsam::Point2 compose(const gtsam::Point2& p2) const; + gtsam::Point2 between(const gtsam::Point2& p2) const; + + // Manifold + gtsam::Point2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Point2& p) const; + + // Lie Group + static gtsam::Point2 Expmap(Vector v); + static Vector Logmap(const gtsam::Point2& p); + + // Standard Interface + double x() const; + double y() const; + Vector vector() const; + double dist(const gtsam::Point2& p2) const; + double norm() const; + + // enabling serialization functionality + void serialize() 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(); +}; + +class StereoPoint2 { + // Standard Constructors + StereoPoint2(); + StereoPoint2(double uL, double uR, double v); + + // Testable + void print(string s) const; + bool equals(const gtsam::StereoPoint2& point, double tol) const; + + // Group + static gtsam::StereoPoint2 identity(); + gtsam::StereoPoint2 inverse() const; + gtsam::StereoPoint2 compose(const gtsam::StereoPoint2& p2) const; + gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2) const; + + // Manifold + gtsam::StereoPoint2 retract(Vector v) const; + Vector localCoordinates(const gtsam::StereoPoint2& p) const; + + // Lie Group + static gtsam::StereoPoint2 Expmap(Vector v); + static Vector Logmap(const gtsam::StereoPoint2& p); + + // Standard Interface + Vector vector() const; + double uL() const; + double uR() const; + double v() const; + + // enabling serialization functionality + void serialize() const; +}; + +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(); + gtsam::Point3 inverse() const; + gtsam::Point3 compose(const gtsam::Point3& p2) const; + gtsam::Point3 between(const gtsam::Point3& p2) const; + + // Manifold + gtsam::Point3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Point3& p) const; + + // Lie Group + static gtsam::Point3 Expmap(Vector v); + static Vector Logmap(const gtsam::Point3& p); + + // Standard Interface + Vector vector() const; + double x() const; + double y() const; + double z() const; + + // enabling serialization functionality + void serialize() const; +}; + +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) 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; +}; + +class Rot3 { + // Standard Constructors and Named Constructors + Rot3(); + Rot3(Matrix R); + static gtsam::Rot3 Rx(double t); + static gtsam::Rot3 Ry(double t); + static gtsam::Rot3 Rz(double t); + static gtsam::Rot3 RzRyRx(double x, double y, double z); + static gtsam::Rot3 RzRyRx(Vector xyz); + static gtsam::Rot3 yaw(double t); // positive yaw is to right (as in aircraft heading) + static gtsam::Rot3 pitch(double t); // positive pitch is up (increasing aircraft altitude) + static gtsam::Rot3 roll(double t); // positive roll is to right (increasing yaw in aircraft) + static gtsam::Rot3 ypr(double y, double p, double r); + static gtsam::Rot3 quaternion(double w, double x, double y, double z); + static gtsam::Rot3 Rodrigues(Vector v); + + // Testable + void print(string s) const; + bool equals(const gtsam::Rot3& rot, double tol) const; + + // Group + static gtsam::Rot3 identity(); + gtsam::Rot3 inverse() const; + gtsam::Rot3 compose(const gtsam::Rot3& p2) const; + gtsam::Rot3 between(const gtsam::Rot3& p2) const; + + // Manifold + //gtsam::Rot3 retractCayley(Vector v) const; // FIXME, does not exist in both Matrix and Quaternion options + gtsam::Rot3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Rot3& p) const; + + // Group Action on Point3 + gtsam::Point3 rotate(const gtsam::Point3& p) const; + gtsam::Point3 unrotate(const gtsam::Point3& p) const; + + // Standard Interface + static gtsam::Rot3 Expmap(Vector v); + static Vector Logmap(const gtsam::Rot3& p); + Matrix matrix() const; + Matrix transpose() const; + gtsam::Point3 column(size_t index) const; + Vector xyz() const; + Vector ypr() const; + Vector rpy() const; + double roll() const; + double pitch() const; + double yaw() const; +// Vector toQuaternion() const; // FIXME: Can't cast to Vector properly + Vector quaternion() const; + + // enabling serialization functionality + void serialize() const; +}; + +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; +}; + +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 +class Pose3Vector +{ + Pose3Vector(); + size_t size() const; + bool empty() const; + gtsam::Pose3 at(size_t n) const; + void push_back(const gtsam::Pose3& x); +}; + +#include +class Unit3 { + // Standard Constructors + Unit3(); + Unit3(const gtsam::Point3& pose); + + // Testable + void print(string s) const; + bool equals(const gtsam::Unit3& pose, double tol) const; + + // Other functionality + Matrix basis() const; + Matrix skew() const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Unit3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Unit3& s) const; +}; + +#include +class EssentialMatrix { + // 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); +}; + +class Cal3_S2 { + // Standard Constructors + Cal3_S2(); + Cal3_S2(double fx, double fy, double s, double u0, double v0); + Cal3_S2(Vector v); + Cal3_S2(double fov, int w, int h); + + // Testable + void print(string s) const; + bool equals(const gtsam::Cal3_S2& rhs, double tol) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Cal3_S2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3_S2& c) const; + + // Action on Point2 + gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + + // Standard Interface + double fx() const; + double fy() const; + double skew() const; + double px() const; + double py() const; + gtsam::Point2 principalPoint() const; + Vector vector() const; + Matrix matrix() const; + Matrix matrix_inverse() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class Cal3DS2_Base { + // Standard Constructors + Cal3DS2_Base(); + + // Testable + void print(string s) const; + + // Standard Interface + double fx() const; + double fy() const; + double skew() const; + double px() const; + double py() const; + double k1() const; + double k2() const; + + // Action on Point2 + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const; + gtsam::Point2 calibrate(const gtsam::Point2& p) const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class Cal3DS2 : gtsam::Cal3DS2_Base { + // Standard Constructors + Cal3DS2(); + Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2); + Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2); + Cal3DS2(Vector v); + + // Testable + bool equals(const gtsam::Cal3DS2& rhs, double tol) const; + + // Manifold + size_t dim() const; + static size_t Dim(); + gtsam::Cal3DS2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3DS2& c) const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class Cal3Unified : gtsam::Cal3DS2_Base { + // Standard Constructors + Cal3Unified(); + Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2); + Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2, double xi); + Cal3Unified(Vector v); + + // Testable + bool equals(const gtsam::Cal3Unified& rhs, double tol) const; + + // Standard Interface + double xi() const; + gtsam::Point2 spaceToNPlane(const gtsam::Point2& p) const; + gtsam::Point2 nPlaneToSpace(const gtsam::Point2& p) const; + + // Manifold + size_t dim() const; + static size_t Dim(); + gtsam::Cal3Unified retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3Unified& c) const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +class Cal3_S2Stereo { + // Standard Constructors + Cal3_S2Stereo(); + Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b); + Cal3_S2Stereo(Vector v); + + // Testable + void print(string s) const; + bool equals(const gtsam::Cal3_S2Stereo& K, double tol) const; + + // Standard Interface + double fx() const; + double fy() const; + double skew() const; + double px() const; + double py() const; + gtsam::Point2 principalPoint() const; + double baseline() const; +}; + +#include +class Cal3Bundler { + // Standard Constructors + Cal3Bundler(); + Cal3Bundler(double fx, double k1, double k2, double u0, double v0); + + // Testable + void print(string s) const; + bool equals(const gtsam::Cal3Bundler& rhs, double tol) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Cal3Bundler retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3Bundler& c) const; + + // Action on Point2 + gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const; + gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + + // Standard Interface + double fx() const; + double fy() const; + double k1() const; + double k2() const; + double u0() const; + double v0() const; + Vector vector() const; + Vector k() const; + //Matrix K() const; //FIXME: Uppercase + + // enabling serialization functionality + void serialize() const; +}; + +class CalibratedCamera { + // Standard Constructors and Named Constructors + CalibratedCamera(); + CalibratedCamera(const gtsam::Pose3& pose); + CalibratedCamera(const Vector& v); + static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height); + + // Testable + void print(string s) const; + bool equals(const gtsam::CalibratedCamera& camera, double tol) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::CalibratedCamera retract(const Vector& d) const; + Vector localCoordinates(const gtsam::CalibratedCamera& T2) const; + + // Action on Point3 + gtsam::Point2 project(const gtsam::Point3& point) const; + static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); + + // Standard Interface + gtsam::Pose3 pose() const; + double range(const gtsam::Point3& p) const; // TODO: Other overloaded range methods + + // enabling serialization functionality + void serialize() const; +}; + +template +class PinholeCamera { + // Standard Constructors and Named Constructors + PinholeCamera(); + PinholeCamera(const gtsam::Pose3& pose); + PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K); + static This Level(const CALIBRATION& K, const gtsam::Pose2& pose, double height); + static This Level(const gtsam::Pose2& pose, double height); + static This Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, + const gtsam::Point3& upVector, const CALIBRATION& K); + + // Testable + void print(string s) const; + bool equals(const This& camera, double tol) const; + + // Standard Interface + gtsam::Pose3 pose() const; + CALIBRATION calibration() const; + + // Manifold + This retract(const Vector& d) const; + Vector localCoordinates(const This& T2) const; + size_t dim() const; + static size_t Dim(); + + // Transformations and measurement functions + static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); + pair projectSafe(const gtsam::Point3& pw) const; + gtsam::Point2 project(const gtsam::Point3& point); + gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; + double range(const gtsam::Point3& point); + double range(const gtsam::Pose3& point); + + // enabling serialization functionality + void serialize() const; +}; + +virtual class SimpleCamera { + // Standard Constructors and Named Constructors + SimpleCamera(); + SimpleCamera(const gtsam::Pose3& pose); + SimpleCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2& K); + static gtsam::SimpleCamera Level(const gtsam::Cal3_S2& K, const gtsam::Pose2& pose, double height); + static gtsam::SimpleCamera Level(const gtsam::Pose2& pose, double height); + static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, + const gtsam::Point3& upVector, const gtsam::Cal3_S2& K); + + // Testable + void print(string s) const; + bool equals(const gtsam::SimpleCamera& camera, double tol) const; + + // Standard Interface + gtsam::Pose3 pose() const; + gtsam::Cal3_S2 calibration() const; + + // Manifold + gtsam::SimpleCamera retract(const Vector& d) const; + Vector localCoordinates(const gtsam::SimpleCamera& T2) const; + size_t dim() const; + static size_t Dim(); + + // Transformations and measurement functions + static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); + pair projectSafe(const gtsam::Point3& pw) const; + gtsam::Point2 project(const gtsam::Point3& point); + gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; + double range(const gtsam::Point3& point); + double range(const gtsam::Pose3& point); + + // enabling serialization functionality + void serialize() const; + +}; + +// Some typedefs for common camera types +// PinholeCameraCal3_S2 is the same as SimpleCamera above +typedef gtsam::PinholeCamera PinholeCameraCal3_S2; +typedef gtsam::PinholeCamera PinholeCameraCal3DS2; +typedef gtsam::PinholeCamera PinholeCameraCal3Unified; +typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; + +class StereoCamera { + // Standard Constructors and Named Constructors + StereoCamera(); + StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K); + + // Testable + void print(string s) const; + bool equals(const gtsam::StereoCamera& camera, double tol) const; + + // Standard Interface + gtsam::Pose3 pose() const; + double baseline() const; + gtsam::Cal3_S2Stereo calibration() const; + + // Manifold + gtsam::StereoCamera retract(const Vector& d) const; + Vector localCoordinates(const gtsam::StereoCamera& T2) const; + size_t dim() const; + static size_t Dim(); + + // Transformations and measurement functions + gtsam::StereoPoint2 project(const gtsam::Point3& point); + gtsam::Point3 backproject(const gtsam::StereoPoint2& p) const; + + // enabling serialization functionality + void serialize() const; +}; + +#include + +// Templates appear not yet supported for free functions +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); + +//************************************************************************* +// 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); + + // 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(); + 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); + 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); + 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); + 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); + 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); + + //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); + + //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 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, 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); + + //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 info() 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); + + // 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); + + //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); + + //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); + + // 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; +}; + +class Errors { + //Constructors + Errors(); + Errors(const gtsam::VectorValues& V); + + //Testable + void print(string s); + bool equals(const gtsam::Errors& expected, double tol) const; +}; + +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) ; + void print() const; +}; + +//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); + void print(); +}; + +#include +virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters { + SubgraphSolverParameters(); + 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); + 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 +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(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; + 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; +}; + +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; +}; + +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 + void 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 +}; + +virtual class NoiseModelFactor: gtsam::NonlinearFactor { + void equals(const gtsam::NoiseModelFactor& other, double tol) const; + gtsam::noiseModel::Base* get_noiseModel() const; + Vector unwhitenedError(const gtsam::Values& x) const; + Vector whitenedError(const gtsam::Values& x) const; +}; + +#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; + + void insert(size_t j, const gtsam::Point2& t); + void insert(size_t j, const gtsam::Point3& t); + void insert(size_t j, const gtsam::Rot2& t); + void insert(size_t j, const gtsam::Pose2& t); + void insert(size_t j, const gtsam::Rot3& t); + void insert(size_t j, const gtsam::Pose3& t); + void insert(size_t j, const gtsam::Cal3_S2& t); + void insert(size_t j, const gtsam::Cal3DS2& t); + void insert(size_t j, const gtsam::Cal3Bundler& t); + void insert(size_t j, const gtsam::EssentialMatrix& t); + void insert(size_t j, const gtsam::SimpleCamera& t); + void insert(size_t j, const gtsam::imuBias::ConstantBias& t); + void insert(size_t j, Vector t); + void insert(size_t j, Matrix t); + + // Fixed size version + void insertFixed(size_t j, Vector t, size_t n); + + void update(size_t j, const gtsam::Point2& t); + void update(size_t j, const gtsam::Point3& t); + void update(size_t j, const gtsam::Rot2& t); + void update(size_t j, const gtsam::Pose2& t); + void update(size_t j, const gtsam::Rot3& t); + void update(size_t j, const gtsam::Pose3& t); + void update(size_t j, const gtsam::Cal3_S2& t); + void update(size_t j, const gtsam::Cal3DS2& t); + void update(size_t j, const gtsam::Cal3Bundler& t); + void update(size_t j, const gtsam::EssentialMatrix& t); + void update(size_t j, const gtsam::imuBias::ConstantBias& t); + void update(size_t j, Vector t); + void update(size_t j, Matrix t); + + template + T at(size_t j); + + /// Fixed size versions, for n in 1..9 + void insertFixed(size_t j, Vector t, size_t n); + + /// Fixed size versions, for n in 1..9 + Vector atFixed(size_t j, size_t n); + + /// version for double + void insertDouble(size_t j, double c); + double atDouble(size_t j) 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); +}; + +#include +class Marginals { + Marginals(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& solution); + + 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; +}; + +class JointMarginal { + Matrix at(size_t iVariable, size_t jVariable) const; + Matrix fullMatrix() const; + void print(string s) const; + void print() 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 +#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 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(); +}; + +#include +virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams { + LevenbergMarquardtParams(); + + 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(); + + double getDeltaInitial() const; + string getVerbosityDL() const; + + void setDeltaInitial(double deltaInitial) const; + void setVerbosityDL(string verbosityDL) const; +}; + +virtual class NonlinearOptimizer { + gtsam::Values optimize(); + gtsam::Values optimizeSafely(); + double error() const; + int iterations() const; + gtsam::Values values() const; + void iterate() const; +}; + +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); +}; + +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; +}; + +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(); + + void print(string str) const; + + /** Getters and Setters for all properties */ + double getWildfireThreshold() const; + void setWildfireThreshold(double wildfireThreshold); +}; + +class ISAM2DoglegParams { + ISAM2DoglegParams(); + + 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(); + + 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(); + + //Standard Interface + Vector gradientContribution() const; + void print(string s); +}; + +class ISAM2Result { + ISAM2Result(); + + void print(string str) const; + + /** Getters and Setters for all properties */ + size_t getVariablesRelinearized() const; + size_t getVariablesReeliminated() const; + size_t getCliques() 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; + 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::KeyVector& removeFactorIndices); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& 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; + gtsam::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; + 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); + 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); + 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); + + // 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); +}; + +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); + + // 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); + + // 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); + + 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; + +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 +class SmartProjectionParams { + SmartProjectionParams(); + // 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); + + 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 +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 { + // Standard Constructor + 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; + + // 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 PoseVelocityBias{ + PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias); + }; +class PreintegratedImuMeasurements { + // Standard Constructor + PreintegratedImuMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration); + PreintegratedImuMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance); + // PreintegratedImuMeasurements(const gtsam::PreintegratedImuMeasurements& rhs); + + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol); + + double deltaTij() const; + gtsam::Rot3 deltaRij() const; + Vector deltaPij() const; + Vector deltaVij() const; + Vector biasHatVector() const; + Matrix delPdelBiasAcc() const; + Matrix delPdelBiasOmega() const; + Matrix delVdelBiasAcc() const; + Matrix delVdelBiasOmega() const; + Matrix delRdelBiasOmega() const; + Matrix preintMeasCov() const; + + // Standard Interface + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); + gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias, + Vector gravity, Vector omegaCoriolis) 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, Vector gravity, Vector omegaCoriolis); + ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias, + const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis, + const gtsam::Pose3& body_P_sensor); + // Standard Interface + gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const; +}; + +#include +class PreintegratedCombinedMeasurements { + // Standard Constructor + PreintegratedCombinedMeasurements( + const gtsam::imuBias::ConstantBias& bias, + Matrix measuredAccCovariance, + Matrix measuredOmegaCovariance, + Matrix integrationErrorCovariance, + Matrix biasAccCovariance, + Matrix biasOmegaCovariance, + Matrix biasAccOmegaInit); + PreintegratedCombinedMeasurements( + const gtsam::imuBias::ConstantBias& bias, + Matrix measuredAccCovariance, + Matrix measuredOmegaCovariance, + Matrix integrationErrorCovariance, + Matrix biasAccCovariance, + Matrix biasOmegaCovariance, + Matrix biasAccOmegaInit, + bool use2ndOrderIntegration); + // PreintegratedCombinedMeasurements(const gtsam::PreintegratedCombinedMeasurements& rhs); + + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, double tol); + + double deltaTij() const; + gtsam::Rot3 deltaRij() const; + Vector deltaPij() const; + Vector deltaVij() const; + Vector biasHatVector() const; + Matrix delPdelBiasAcc() const; + Matrix delPdelBiasOmega() const; + Matrix delVdelBiasAcc() const; + Matrix delVdelBiasOmega() const; + Matrix delRdelBiasOmega() const; + Matrix preintMeasCov() const; + + // Standard Interface + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); + gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias, + Vector gravity, Vector omegaCoriolis) 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, Vector gravity, Vector omegaCoriolis); + // Standard Interface + gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const; +}; + +#include +class PreintegratedAhrsMeasurements { + // Standard Constructor + PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance); + 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); + + // 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; + 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; + 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 + +} From 4d93a33f610334068b6ea71458d2a7d788ef4c2a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 26 Jan 2016 23:09:58 -0800 Subject: [PATCH 2/6] Static methods should be uppercase. --- .gitignore | 1 + examples/CreateSFMExampleData.cpp | 6 +- gtsam/geometry/Rot3.h | 19 ++++-- gtsam/geometry/tests/testCalibratedCamera.cpp | 4 +- gtsam/geometry/tests/testEssentialMatrix.cpp | 10 +-- gtsam/geometry/tests/testOrientedPlane3.cpp | 2 +- gtsam/geometry/tests/testPinholeCamera.cpp | 2 +- gtsam/geometry/tests/testPinholePose.cpp | 2 +- gtsam/geometry/tests/testPose3.cpp | 16 ++--- gtsam/geometry/tests/testRot3.cpp | 22 +++--- gtsam/geometry/tests/testTriangulation.cpp | 10 +-- gtsam/geometry/tests/testUnit3.cpp | 4 +- gtsam/navigation/GPSFactor.cpp | 4 +- gtsam/navigation/MagFactor.h | 2 +- gtsam/navigation/tests/testAHRSFactor.cpp | 2 +- .../tests/testCombinedImuFactor.cpp | 4 +- gtsam/navigation/tests/testGPSFactor.cpp | 2 +- gtsam/navigation/tests/testImuFactor.cpp | 10 +-- gtsam/navigation/tests/testMagFactor.cpp | 2 +- gtsam/slam/dataset.cpp | 4 +- gtsam/slam/tests/smartFactorScenarios.h | 4 +- gtsam/slam/tests/testDataset.cpp | 2 +- gtsam/slam/tests/testOrientedPlane3Factor.cpp | 4 +- gtsam/slam/tests/testPoseRotationPrior.cpp | 2 +- gtsam/slam/tests/testPoseTranslationPrior.cpp | 2 +- .../tests/testSmartProjectionCameraFactor.cpp | 4 +- .../tests/testSmartProjectionPoseFactor.cpp | 56 +++++++-------- gtsam/slam/tests/testTriangulationFactor.cpp | 2 +- gtsam_unstable/dynamics/PoseRTV.cpp | 2 +- .../dynamics/tests/testIMUSystem.cpp | 12 ++-- gtsam_unstable/dynamics/tests/testPoseRTV.cpp | 8 +-- .../dynamics/tests/testSimpleHelicopter.cpp | 2 +- gtsam_unstable/geometry/Pose3Upright.cpp | 2 +- .../geometry/tests/testInvDepthCamera3.cpp | 4 +- .../geometry/tests/testPose3Upright.cpp | 4 +- .../geometry/tests/testSimilarity3.cpp | 36 +++++----- gtsam_unstable/slam/Mechanization_bRn2.cpp | 2 +- .../slam/tests/testInvDepthFactor3.cpp | 2 +- .../slam/tests/testInvDepthFactorVariant1.cpp | 4 +- .../slam/tests/testInvDepthFactorVariant2.cpp | 4 +- .../slam/tests/testInvDepthFactorVariant3.cpp | 4 +- .../tests/testRelativeElevationFactor.cpp | 2 +- .../testSmartStereoProjectionPoseFactor.cpp | 68 +++++++++---------- 43 files changed, 184 insertions(+), 176 deletions(-) diff --git a/.gitignore b/.gitignore index d46bddd10..7850df41b 100644 --- a/.gitignore +++ b/.gitignore @@ -6,3 +6,4 @@ /examples/Data/pose3example-rewritten.txt *.txt.user *.txt.user.6d59f0c +/python-build/ diff --git a/examples/CreateSFMExampleData.cpp b/examples/CreateSFMExampleData.cpp index bcc0b6320..010f474bf 100644 --- a/examples/CreateSFMExampleData.cpp +++ b/examples/CreateSFMExampleData.cpp @@ -35,7 +35,7 @@ void createExampleBALFile(const string& filename, const vector& P, SfM_data data; // Create two cameras - Rot3 aRb = Rot3::yaw(M_PI_2); + Rot3 aRb = Rot3::Yaw(M_PI_2); Point3 aTb(0.1, 0, 0); Pose3 identity, aPb(aRb, aTb); data.cameras.push_back(SfM_Camera(pose1, K)); @@ -66,7 +66,7 @@ void createExampleBALFile(const string& filename, const vector& P, void create5PointExample1() { // Create two cameras poses - Rot3 aRb = Rot3::yaw(M_PI_2); + Rot3 aRb = Rot3::Yaw(M_PI_2); Point3 aTb(0.1, 0, 0); Pose3 pose1, pose2(aRb, aTb); @@ -85,7 +85,7 @@ void create5PointExample1() { void create5PointExample2() { // Create two cameras poses - Rot3 aRb = Rot3::yaw(M_PI_2); + Rot3 aRb = Rot3::Yaw(M_PI_2); Point3 aTb(10, 0, 0); Pose3 pose1, pose2(aRb, aTb); diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index d8b8a4682..4dd582a77 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -146,13 +146,13 @@ namespace gtsam { } /// Positive yaw is to right (as in aircraft heading). See ypr - static Rot3 yaw (double t) { return Rz(t); } + static Rot3 Yaw (double t) { return Rz(t); } /// Positive pitch is up (increasing aircraft altitude).See ypr - static Rot3 pitch(double t) { return Ry(t); } + static Rot3 Pitch(double t) { return Ry(t); } //// Positive roll is to right (increasing yaw in aircraft). - static Rot3 roll (double t) { return Rx(t); } + static Rot3 Roll (double t) { return Rx(t); } /** * Returns rotation nRb from body to nav frame. @@ -163,7 +163,7 @@ namespace gtsam { * as described in http://www.sedris.org/wg8home/Documents/WG80462.pdf. * Assumes vehicle coordinate frame X forward, Y right, Z down. */ - static Rot3 ypr(double y, double p, double r) { return RzRyRx(r,p,y);} + static Rot3 Ypr(double y, double p, double r) { return RzRyRx(r,p,y);} /** Create from Quaternion coefficients */ static Rot3 quaternion(double w, double x, double y, double z) { @@ -419,13 +419,13 @@ namespace gtsam { /** * Use RQ to calculate yaw-pitch-roll angle representation - * @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r) + * @return a vector containing ypr s.t. R = Rot3::Ypr(y,p,r) */ Vector3 ypr() const; /** * Use RQ to calculate roll-pitch-yaw angle representation - * @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r) + * @return a vector containing ypr s.t. R = Rot3::Ypr(y,p,r) */ Vector3 rpy() const; @@ -488,6 +488,13 @@ namespace gtsam { static Rot3 rodriguez(double wx, double wy, double wz) { return Rodrigues(wx, wy, wz); } /// @} +#ifdef ALLOW_DEPRECATED_IN_GTSAM4 + static Rot3 yaw (double t) { return Yaw(t); } + static Rot3 pitch(double t) { return Pitch(t); } + static Rot3 roll (double t) { return Roll(t); } + static Rot3 ypr(double y, double p, double r) { return Ypr(r,p,y);} +#endif + private: /** Serialization function */ diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index 3b0906b44..5bc645a58 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -135,7 +135,7 @@ TEST( CalibratedCamera, Dproject_point_pose) // Add a test with more arbitrary rotation TEST( CalibratedCamera, Dproject_point_pose2) { - static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); + static const Pose3 pose1(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); static const CalibratedCamera camera(pose1); Matrix Dpose, Dpoint; camera.project(point1, Dpose, Dpoint); @@ -165,7 +165,7 @@ TEST( CalibratedCamera, Dproject_point_pose_infinity) // Add a test with more arbitrary rotation TEST( CalibratedCamera, Dproject_point_pose2_infinity) { - static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); + static const Pose3 pose1(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); static const CalibratedCamera camera(pose1); Matrix Dpose, Dpoint; camera.project2(pointAtInfinity, Dpose, Dpoint); diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index fe27b2911..bfff0a182 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -20,7 +20,7 @@ GTSAM_CONCEPT_MANIFOLD_INST(EssentialMatrix) //************************************************************************* // Create two cameras and corresponding essential matrix E -Rot3 c1Rc2 = Rot3::yaw(M_PI_2); +Rot3 c1Rc2 = Rot3::Yaw(M_PI_2); Point3 c1Tc2(0.1, 0, 0); EssentialMatrix trueE(c1Rc2, Unit3(c1Tc2)); @@ -98,8 +98,8 @@ Point3 transform_to_(const EssentialMatrix& E, const Point3& point) { } TEST (EssentialMatrix, transform_to) { // test with a more complicated EssentialMatrix - Rot3 aRb2 = Rot3::yaw(M_PI / 3.0) * Rot3::pitch(M_PI_4) - * Rot3::roll(M_PI / 6.0); + Rot3 aRb2 = Rot3::Yaw(M_PI / 3.0) * Rot3::Pitch(M_PI_4) + * Rot3::Roll(M_PI / 6.0); Point3 aTb2(19.2, 3.7, 5.9); EssentialMatrix E(aRb2, Unit3(aTb2)); //EssentialMatrix E(aRb, Unit3(aTb).retract(Vector2(0.1, 0))); @@ -159,7 +159,7 @@ TEST (EssentialMatrix, FromPose3_a) { //************************************************************************* TEST (EssentialMatrix, FromPose3_b) { Matrix actualH; - Rot3 c1Rc2 = Rot3::ypr(0.1, -0.2, 0.3); + Rot3 c1Rc2 = Rot3::Ypr(0.1, -0.2, 0.3); Point3 c1Tc2(0.4, 0.5, 0.6); EssentialMatrix E(c1Rc2, Unit3(c1Tc2)); Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras @@ -181,7 +181,7 @@ TEST (EssentialMatrix, streaming) { //************************************************************************* TEST (EssentialMatrix, epipoles) { // Create an E - Rot3 c1Rc2 = Rot3::ypr(0.1, -0.2, 0.3); + Rot3 c1Rc2 = Rot3::Ypr(0.1, -0.2, 0.3); Point3 c1Tc2(0.4, 0.5, 0.6); EssentialMatrix E(c1Rc2, Unit3(c1Tc2)); diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index 11931449f..180abb0d6 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -59,7 +59,7 @@ OrientedPlane3 transform_(const OrientedPlane3& plane, const Pose3& xr) { } TEST (OrientedPlane3, transform) { - gtsam::Pose3 pose(gtsam::Rot3::ypr(-M_PI / 4.0, 0.0, 0.0), + gtsam::Pose3 pose(gtsam::Rot3::Ypr(-M_PI / 4.0, 0.0, 0.0), gtsam::Point3(2.0, 3.0, 4.0)); OrientedPlane3 plane(-1, 0, 0, 5); OrientedPlane3 expectedPlane(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, 0.0, 3); diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 74bc4ca2a..7293d4235 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -242,7 +242,7 @@ TEST( PinholeCamera, Dproject2) // Add a test with more arbitrary rotation TEST( PinholeCamera, Dproject3) { - static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); + static const Pose3 pose1(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); static const Camera camera(pose1); Matrix Dpose, Dpoint; camera.project2(point1, Dpose, Dpoint); diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index dc294899f..8f3eadc51 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -172,7 +172,7 @@ TEST( PinholePose, Dproject2) // Add a test with more arbitrary rotation TEST( CalibratedCamera, Dproject3) { - static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); + static const Pose3 pose1(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); static const Camera camera(pose1); Matrix Dpose, Dpoint; camera.project2(point1, Dpose, Dpoint); diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 9007ce1bd..fb3907df3 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -556,12 +556,12 @@ TEST( Pose3, between ) /* ************************************************************************* */ // some shared test values - pulled from equivalent test in Pose2 Point3 l1(1, 0, 0), l2(1, 1, 0), l3(2, 2, 0), l4(1, 4,-4); -Pose3 x1, x2(Rot3::ypr(0.0, 0.0, 0.0), l2), x3(Rot3::ypr(M_PI/4.0, 0.0, 0.0), l2); +Pose3 x1, x2(Rot3::Ypr(0.0, 0.0, 0.0), l2), x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2); Pose3 - xl1(Rot3::ypr(0.0, 0.0, 0.0), Point3(1, 0, 0)), - xl2(Rot3::ypr(0.0, 1.0, 0.0), Point3(1, 1, 0)), - xl3(Rot3::ypr(1.0, 0.0, 0.0), Point3(2, 2, 0)), - xl4(Rot3::ypr(0.0, 0.0, 1.0), Point3(1, 4,-4)); + xl1(Rot3::Ypr(0.0, 0.0, 0.0), Point3(1, 0, 0)), + xl2(Rot3::Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0)), + xl3(Rot3::Ypr(1.0, 0.0, 0.0), Point3(2, 2, 0)), + xl4(Rot3::Ypr(0.0, 0.0, 1.0), Point3(1, 4,-4)); /* ************************************************************************* */ double range_proxy(const Pose3& pose, const Point3& point) { @@ -654,9 +654,9 @@ TEST( Pose3, unicycle ) { // velocity in X should be X in inertial frame, rather than global frame Vector x_step = delta(6,3,1.0); - EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), l1), expmap_default(x1, x_step), tol)); - EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), Point3(2,1,0)), expmap_default(x2, x_step), tol)); - EXPECT(assert_equal(Pose3(Rot3::ypr(M_PI/4.0,0,0), Point3(2,2,0)), expmap_default(x3, sqrt(2.0) * x_step), tol)); + EXPECT(assert_equal(Pose3(Rot3::Ypr(0,0,0), l1), expmap_default(x1, x_step), tol)); + EXPECT(assert_equal(Pose3(Rot3::Ypr(0,0,0), Point3(2,1,0)), expmap_default(x2, x_step), tol)); + EXPECT(assert_equal(Pose3(Rot3::Ypr(M_PI/4.0,0,0), Point3(2,2,0)), expmap_default(x3, sqrt(2.0) * x_step), tol)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index a61467b82..296bd2b7c 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -501,17 +501,17 @@ TEST( Rot3, yaw_pitch_roll ) double t = 0.1; // yaw is around z axis - CHECK(assert_equal(Rot3::Rz(t),Rot3::yaw(t))); + CHECK(assert_equal(Rot3::Rz(t),Rot3::Yaw(t))); // pitch is around y axis - CHECK(assert_equal(Rot3::Ry(t),Rot3::pitch(t))); + CHECK(assert_equal(Rot3::Ry(t),Rot3::Pitch(t))); // roll is around x axis - CHECK(assert_equal(Rot3::Rx(t),Rot3::roll(t))); + CHECK(assert_equal(Rot3::Rx(t),Rot3::Roll(t))); // Check compound rotation - Rot3 expected = Rot3::yaw(0.1) * Rot3::pitch(0.2) * Rot3::roll(0.3); - CHECK(assert_equal(expected,Rot3::ypr(0.1,0.2,0.3))); + Rot3 expected = Rot3::Yaw(0.1) * Rot3::Pitch(0.2) * Rot3::Roll(0.3); + CHECK(assert_equal(expected,Rot3::Ypr(0.1,0.2,0.3))); CHECK(assert_equal((Vector)Vector3(0.1, 0.2, 0.3),expected.ypr())); } @@ -531,14 +531,14 @@ TEST( Rot3, RQ) CHECK(assert_equal(expected,R.xyz(),1e-6)); CHECK(assert_equal((Vector)Vector3(0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz())); - // Try using ypr call, asserting that Rot3::ypr(y,p,r).ypr()==[y;p;r] - CHECK(assert_equal((Vector)Vector3(0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr())); - CHECK(assert_equal((Vector)Vector3(0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy())); + // Try using ypr call, asserting that Rot3::Ypr(y,p,r).ypr()==[y;p;r] + CHECK(assert_equal((Vector)Vector3(0.1,0.2,0.3),Rot3::Ypr(0.1,0.2,0.3).ypr())); + CHECK(assert_equal((Vector)Vector3(0.3,0.2,0.1),Rot3::Ypr(0.1,0.2,0.3).rpy())); // Try ypr for pure yaw-pitch-roll matrices - CHECK(assert_equal((Vector)Vector3(0.1,0.0,0.0),Rot3::yaw (0.1).ypr())); - CHECK(assert_equal((Vector)Vector3(0.0,0.1,0.0),Rot3::pitch(0.1).ypr())); - CHECK(assert_equal((Vector)Vector3(0.0,0.0,0.1),Rot3::roll (0.1).ypr())); + CHECK(assert_equal((Vector)Vector3(0.1,0.0,0.0),Rot3::Yaw (0.1).ypr())); + CHECK(assert_equal((Vector)Vector3(0.0,0.1,0.0),Rot3::Pitch(0.1).ypr())); + CHECK(assert_equal((Vector)Vector3(0.0,0.0,0.1),Rot3::Roll (0.1).ypr())); // Try RQ to recover calibration from 3*3 sub-block of projection matrix Matrix K = (Matrix(3, 3) << 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0).finished(); diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index bd18143cb..c3df95abc 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -40,7 +40,7 @@ static const boost::shared_ptr sharedCal = // boost::make_shared(1500, 1200, 0, 640, 480); // Looking along X-axis, 1 meter above ground plane (x-y) -static const Rot3 upright = Rot3::ypr(-M_PI / 2, 0., -M_PI / 2); +static const Rot3 upright = Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2); static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1)); PinholeCamera camera1(pose1, *sharedCal); @@ -150,7 +150,7 @@ TEST( triangulation, fourPoses) { EXPECT(assert_equal(landmark, *actual2, 1e-2)); // 3. Add a slightly rotated third camera above, again with measurement noise - Pose3 pose3 = pose1 * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); + Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); SimpleCamera camera3(pose3, *sharedCal); Point2 z3 = camera3.project(landmark); @@ -167,7 +167,7 @@ TEST( triangulation, fourPoses) { EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2)); // 4. Test failure: Add a 4th camera facing the wrong way - Pose3 pose4 = Pose3(Rot3::ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera camera4(pose4, *sharedCal); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION @@ -214,7 +214,7 @@ TEST( triangulation, fourPoses_distinct_Ks) { EXPECT(assert_equal(landmark, *actual2, 1e-2)); // 3. Add a slightly rotated third camera above, again with measurement noise - Pose3 pose3 = pose1 * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); + Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); Cal3_S2 K3(700, 500, 0, 640, 480); SimpleCamera camera3(pose3, K3); Point2 z3 = camera3.project(landmark); @@ -232,7 +232,7 @@ TEST( triangulation, fourPoses_distinct_Ks) { EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2)); // 4. Test failure: Add a 4th camera facing the wrong way - Pose3 pose4 = Pose3(Rot3::ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); Cal3_S2 K4(700, 500, 0, 640, 480); SimpleCamera camera4(pose4, K4); diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 3cfffa0da..dbe315807 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -70,7 +70,7 @@ static Unit3 rotate_(const Rot3& R, const Unit3& p) { } TEST(Unit3, rotate) { - Rot3 R = Rot3::yaw(0.5); + Rot3 R = Rot3::Yaw(0.5); Unit3 p(1, 0, 0); Unit3 expected = Unit3(R.column(1)); Unit3 actual = R * p; @@ -95,7 +95,7 @@ static Unit3 unrotate_(const Rot3& R, const Unit3& p) { } TEST(Unit3, unrotate) { - Rot3 R = Rot3::yaw(-M_PI / 4.0); + Rot3 R = Rot3::Yaw(-M_PI / 4.0); Unit3 p(1, 0, 0); Unit3 expected = Unit3(1, 1, 0); Unit3 actual = R.unrotate(p); diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp index bfd3ebb52..87913cda6 100644 --- a/gtsam/navigation/GPSFactor.cpp +++ b/gtsam/navigation/GPSFactor.cpp @@ -58,10 +58,10 @@ pair GPSFactor::EstimateState(double t1, const Point3& NED1, // Estimate Rotation double yaw = atan2(nV.y(), nV.x()); - Rot3 nRy = Rot3::yaw(yaw); // yaw frame + Rot3 nRy = Rot3::Yaw(yaw); // yaw frame Point3 yV = nRy.inverse() * nV; // velocity in yaw frame double pitch = -atan2(yV.z(), yV.x()), roll = 0; - Rot3 nRb = Rot3::ypr(yaw, pitch, roll); + Rot3 nRb = Rot3::Ypr(yaw, pitch, roll); // Construct initial pose Pose3 nTb(nRb, nT); // nTb diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index f70bec8c6..fc1e69190 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -60,7 +60,7 @@ public: static Point3 unrotate(const Rot2& R, const Point3& p, boost::optional HR = boost::none) { - Point3 q = Rot3::yaw(R.theta()).unrotate(p, HR, boost::none); + Point3 q = Rot3::Yaw(R.theta()).unrotate(p, HR, boost::none); if (HR) { // assign to temporary first to avoid error in Win-Debug mode Matrix H = HR->col(2); diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index 2121eda35..02911acb1 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -423,7 +423,7 @@ TEST (AHRSFactor, predictTest) { // Predict Rot3 x; - Rot3 expectedRot = Rot3().ypr(20*M_PI, 0, 0); + Rot3 expectedRot = Rot3::Ypr(20*M_PI, 0, 0); Rot3 actualRot = factor.predict(x, bias, pim, kZeroOmegaCoriolis); EXPECT(assert_equal(expectedRot, actualRot, 1e-6)); diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index d473207da..e3d366d55 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -293,10 +293,10 @@ TEST(CombinedImuFactor, PredictRotation) { gravity, omegaCoriolis); // Predict - Pose3 x(Rot3().ypr(0, 0, 0), Point3(0, 0, 0)), x2; + Pose3 x(Rot3::Ypr(0, 0, 0), Point3(0, 0, 0)), x2; Vector3 v(0, 0, 0), v2; CombinedImuFactor::Predict(x, v, x2, v2, bias, pim, gravity, omegaCoriolis); - Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0)); + Pose3 expectedPose(Rot3::Ypr(M_PI / 10, 0, 0), Point3(0, 0, 0)); EXPECT(assert_equal(expectedPose, x2, tol)); } diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index 8c93020c9..6149c1651 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -89,7 +89,7 @@ TEST(GPSData, init) { // Check values values EXPECT(assert_equal((Vector )Vector3(29.9575, -29.0564, -1.95993), nV, 1e-4)); - EXPECT( assert_equal(Rot3::ypr(-0.770131, 0.046928, 0), T.rotation(), 1e-5)); + EXPECT( assert_equal(Rot3::Ypr(-0.770131, 0.046928, 0), T.rotation(), 1e-5)); Point3 expectedT(2.38461, -2.31289, -0.156011); EXPECT(assert_equal(expectedT, T.translation(), 1e-5)); } diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 25a6e732c..b7893281d 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -813,7 +813,7 @@ TEST(ImuFactor, PredictRotation) { Vector3 v2; ImuFactor::Predict(x1, v1, x2, v2, bias, pim, kGravityAlongNavZDown, kZeroOmegaCoriolis); - Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0)); + Pose3 expectedPose(Rot3::Ypr(M_PI / 10, 0, 0), Point3(0, 0, 0)); Vector3 expectedVelocity; expectedVelocity << 0, 0, 0; EXPECT(assert_equal(expectedPose, x2)); @@ -891,7 +891,7 @@ TEST(ImuFactor, bodyPSensorNoBias) { double dt = 0.001; // Rotate sensor (z-down) to body (same as navigation) i.e. z-up - Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3(0, 0, 0)); + Pose3 body_P_sensor(Rot3::Ypr(0, 0, M_PI), Point3(0, 0, 0)); ImuFactor::PreintegratedMeasurements pim(bias, Z_3x3, Z_3x3, Z_3x3, true); @@ -907,7 +907,7 @@ TEST(ImuFactor, bodyPSensorNoBias) { PoseVelocityBias poseVelocity = pim.predict(x1, v1, bias, n_gravity, omegaCoriolis); - Pose3 expectedPose(Rot3().ypr(-M_PI / 10, 0, 0), Point3(0, 0, 0)); + Pose3 expectedPose(Rot3::Ypr(-M_PI / 10, 0, 0), Point3(0, 0, 0)); EXPECT(assert_equal(expectedPose, poseVelocity.pose)); Vector3 expectedVelocity(0, 0, 0); @@ -942,7 +942,7 @@ TEST(ImuFactor, bodyPSensorWithBias) { // table exerts an equal and opposite force w.r.t gravity Vector3 measuredAcc(0, 0, -9.81); - Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3()); + Pose3 body_P_sensor(Rot3::Ypr(0, 0, M_PI), Point3()); Matrix3 accCov = 1e-7 * I_3x3; Matrix3 gyroCov = 1e-8 * I_3x3; @@ -1025,7 +1025,7 @@ TEST(ImuFactor, serialization) { using namespace gtsam::serializationTestHelpers; Vector3 n_gravity(0, 0, -9.81); - Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3()); + Pose3 body_P_sensor(Rot3::Ypr(0, 0, M_PI), Point3()); Matrix3 accCov = 1e-7 * I_3x3; Matrix3 gyroCov = 1e-8 * I_3x3; Matrix3 integrationCov = 1e-9 * I_3x3; diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 950c6ce63..38aecfcbc 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -40,7 +40,7 @@ Point3 nM(22653.29982, -1956.83010, 44202.47862); // Let's assume scale factor, double scale = 255.0 / 50000.0; // ...ground truth orientation, -Rot3 nRb = Rot3::yaw(-0.1); +Rot3 nRb = Rot3::Yaw(-0.1); Rot2 theta = nRb.yaw(); // ...and bias Point3 bias(10, -10, 50); diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 68c27e76b..9e71e3753 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -510,7 +510,7 @@ GraphAndValues load3D(const string& filename) { Key id; double x, y, z, roll, pitch, yaw; ls >> id >> x >> y >> z >> roll >> pitch >> yaw; - Rot3 R = Rot3::ypr(yaw,pitch,roll); + Rot3 R = Rot3::Ypr(yaw,pitch,roll); Point3 t = Point3(x, y, z); initial->insert(id, Pose3(R,t)); } @@ -537,7 +537,7 @@ GraphAndValues load3D(const string& filename) { Key id1, id2; double x, y, z, roll, pitch, yaw; ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw; - Rot3 R = Rot3::ypr(yaw,pitch,roll); + Rot3 R = Rot3::Ypr(yaw,pitch,roll); Point3 t = Point3(x, y, z); Matrix m = eye(6); for (int i = 0; i < 6; i++) diff --git a/gtsam/slam/tests/smartFactorScenarios.h b/gtsam/slam/tests/smartFactorScenarios.h index 8e83ec503..ccad83f01 100644 --- a/gtsam/slam/tests/smartFactorScenarios.h +++ b/gtsam/slam/tests/smartFactorScenarios.h @@ -34,7 +34,7 @@ Point3 landmark4(10, 0.5, 1.2); Point3 landmark5(10, -0.5, 1.2); // First camera pose, looking along X-axis, 1 meter above ground plane (x-y) -Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); +Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); // Second camera 1 meter to the right of first camera Pose3 pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); // Third camera 1 meter above the first camera @@ -123,7 +123,7 @@ Camera cam3(pose_above, sharedBundlerK); template CAMERA perturbCameraPose(const CAMERA& camera) { - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); Pose3 cameraPose = camera.pose(); Pose3 perturbedCameraPose = cameraPose.compose(noise_pose); diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 0406c3d27..26a00360e 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -452,7 +452,7 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){ SfM_data readData; readBAL(filenameToRead, readData); - Pose3 poseChange = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.3,0.1,0.3)); + Pose3 poseChange = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.3,0.1,0.3)); Values value; for(size_t i=0; i < readData.number_cameras(); i++){ // for each camera diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index 4a7b4c3fe..f1e830d03 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -47,7 +47,7 @@ TEST (OrientedPlane3Factor, lm_translation_error) { // Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose Symbol init_sym('x', 0); - Pose3 init_pose(Rot3::ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); + Pose3 init_pose(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); Vector sigmas(6); sigmas << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001; PriorFactor pose_prior(init_sym, init_pose, @@ -94,7 +94,7 @@ TEST (OrientedPlane3Factor, lm_rotation_error) { // Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose Symbol init_sym('x', 0); - Pose3 init_pose(Rot3::ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); + Pose3 init_pose(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); PriorFactor pose_prior(init_sym, init_pose, noiseModel::Diagonal::Sigmas( (Vector(6) << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001).finished())); diff --git a/gtsam/slam/tests/testPoseRotationPrior.cpp b/gtsam/slam/tests/testPoseRotationPrior.cpp index db04a74eb..3c7d5f2b2 100644 --- a/gtsam/slam/tests/testPoseRotationPrior.cpp +++ b/gtsam/slam/tests/testPoseRotationPrior.cpp @@ -30,7 +30,7 @@ const gtsam::Key poseKey = 1; // Pose3 examples const Point3 point3A(1.0, 2.0, 3.0), point3B(4.0, 6.0, 8.0); -const Rot3 rot3A, rot3B = Rot3::pitch(-M_PI_2), rot3C = Rot3::Expmap(Vector3(0.1, 0.2, 0.3)); +const Rot3 rot3A, rot3B = Rot3::Pitch(-M_PI_2), rot3C = Rot3::Expmap(Vector3(0.1, 0.2, 0.3)); // Pose2 examples const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0); diff --git a/gtsam/slam/tests/testPoseTranslationPrior.cpp b/gtsam/slam/tests/testPoseTranslationPrior.cpp index 2f39701f7..2fd471c9c 100644 --- a/gtsam/slam/tests/testPoseTranslationPrior.cpp +++ b/gtsam/slam/tests/testPoseTranslationPrior.cpp @@ -27,7 +27,7 @@ const gtsam::Key poseKey = 1; // Pose3 examples const Point3 point3A(1.0, 2.0, 3.0), point3B(4.0, 6.0, 8.0); -const Rot3 rot3A, rot3B = Rot3::pitch(-M_PI_2), rot3C = Rot3::RzRyRx(0.1, 0.2, 0.3); +const Rot3 rot3A, rot3B = Rot3::Pitch(-M_PI_2), rot3C = Rot3::RzRyRx(0.1, 0.2, 0.3); // Pose2 examples const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0); diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index 54bbd6c22..467aefe91 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -47,7 +47,7 @@ template PinholeCamera perturbCameraPoseAndCalibration( const PinholeCamera& camera) { GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); Pose3 cameraPose = camera.pose(); Pose3 perturbedCameraPose = cameraPose.compose(noise_pose); @@ -61,7 +61,7 @@ PinholeCamera perturbCameraPoseAndCalibration( /* ************************************************************************* */ TEST( SmartProjectionCameraFactor, perturbCameraPose) { using namespace vanilla; - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); Pose3 perturbed_level_pose = level_pose.compose(noise_pose); Camera actualCamera(perturbed_level_pose, K2); diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 0e2429840..1c1bc3c03 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -163,7 +163,7 @@ TEST( SmartProjectionPoseFactor, noisy ) { Values values; values.insert(x1, cam1.pose()); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); values.insert(x2, pose_right.compose(noise_pose)); @@ -196,7 +196,7 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 cameraPose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); // body poses + Pose3 cameraPose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); // body poses Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1,0,0)); Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0,-1,0)); @@ -205,7 +205,7 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ SimpleCamera cam3(cameraPose3, *K); // create arbitrary body_Pose_sensor (transforms from sensor to body) - Pose3 sensor_to_body = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); // + Pose3 sensor_to_body = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); // // These are the poses we want to estimate, from camera measurements Pose3 bodyPose1 = cameraPose1.compose(sensor_to_body.inverse()); @@ -263,7 +263,7 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ double expectedError = 0.0; DOUBLES_EQUAL(expectedError, actualError, 1e-7) - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); Values values; values.insert(x1, bodyPose1); values.insert(x2, bodyPose2); @@ -317,8 +317,8 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { groundTruth.insert(x3, cam3.pose()); DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); @@ -539,8 +539,8 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); @@ -606,8 +606,8 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); @@ -667,8 +667,8 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); @@ -792,7 +792,7 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) { graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); @@ -844,7 +844,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { graph.push_back(PriorFactor(x1, level_pose, noisePrior)); graph.push_back(PriorFactor(x2, pose_right, noisePrior)); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); Values values; values.insert(x1, level_pose); @@ -908,8 +908,8 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { graph.push_back(smartFactor2); graph.push_back(smartFactor3); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); @@ -995,7 +995,7 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac graph.push_back( PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); @@ -1063,8 +1063,8 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) graph.push_back( PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); @@ -1108,7 +1108,7 @@ TEST( SmartProjectionPoseFactor, Hessian ) { SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2)); smartFactor1->add(measurements_cam1, views); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); Values values; values.insert(x1, cam1.pose()); @@ -1148,7 +1148,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) { boost::shared_ptr factor = smartFactorInstance->linearize( values); - Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); + Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; rotValues.insert(x1, poseDrift.compose(level_pose)); @@ -1161,7 +1161,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) { // Hessian is invariant to rotations in the nondegenerate case EXPECT(assert_equal(factor->information(), factorRot->information(), 1e-7)); - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), + Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), Point3(10, -4, 5)); Values tranValues; @@ -1203,7 +1203,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { boost::shared_ptr factor = smartFactor->linearize(values); - Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); + Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; rotValues.insert(x1, poseDrift.compose(level_pose)); @@ -1216,7 +1216,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { // Hessian is invariant to rotations in the nondegenerate case EXPECT(assert_equal(factor->information(), factorRot->information(), 1e-7)); - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), + Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), Point3(10, -4, 5)); Values tranValues; @@ -1278,8 +1278,8 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) { graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); @@ -1357,8 +1357,8 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { graph.push_back( PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); diff --git a/gtsam/slam/tests/testTriangulationFactor.cpp b/gtsam/slam/tests/testTriangulationFactor.cpp index 5ac92b4a9..1d2baefee 100644 --- a/gtsam/slam/tests/testTriangulationFactor.cpp +++ b/gtsam/slam/tests/testTriangulationFactor.cpp @@ -37,7 +37,7 @@ static const boost::shared_ptr sharedCal = // boost::make_shared(1500, 1200, 0, 640, 480); // Looking along X-axis, 1 meter above ground plane (x-y) -static const Rot3 upright = Rot3::ypr(-M_PI / 2, 0., -M_PI / 2); +static const Rot3 upright = Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2); static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1)); SimpleCamera camera1(pose1, *sharedCal); diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index 51737285a..1a7fdf3de 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -100,7 +100,7 @@ PoseRTV PoseRTV::flyingDynamics( double pitch2 = r2.pitch(); double forward_accel = -thrust * sin(pitch2); // r2, pitch (in global frame?) controls forward force double loss_lift = lift*fabs(sin(pitch2)); - Rot3 yaw_correction_bn = Rot3::yaw(yaw2); + Rot3 yaw_correction_bn = Rot3::Yaw(yaw2); Point3 forward(forward_accel, 0.0, 0.0); Vector Acc_n = yaw_correction_bn.rotate(forward).vector() // applies locally forward force in the global frame diff --git a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp index 3fff06de1..3fc6a0197 100644 --- a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp +++ b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp @@ -56,9 +56,9 @@ TEST( testIMUSystem, optimize_chain ) { // create a simple chain of poses to generate IMU measurements const double dt = 1.0; PoseRTV pose1, - pose2(Point3(1.0, 1.0, 0.0), Rot3::ypr(0.1, 0.0, 0.0), Velocity3(2.0, 2.0, 0.0)), - pose3(Point3(2.0, 2.0, 0.0), Rot3::ypr(0.2, 0.0, 0.0), Velocity3(0.0, 0.0, 0.0)), - pose4(Point3(3.0, 3.0, 0.0), Rot3::ypr(0.3, 0.0, 0.0), Velocity3(2.0, 2.0, 0.0)); + pose2(Point3(1.0, 1.0, 0.0), Rot3::Ypr(0.1, 0.0, 0.0), Velocity3(2.0, 2.0, 0.0)), + pose3(Point3(2.0, 2.0, 0.0), Rot3::Ypr(0.2, 0.0, 0.0), Velocity3(0.0, 0.0, 0.0)), + pose4(Point3(3.0, 3.0, 0.0), Rot3::Ypr(0.3, 0.0, 0.0), Velocity3(2.0, 2.0, 0.0)); // create measurements SharedDiagonal model = noiseModel::Unit::Create(6); @@ -102,9 +102,9 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) { // create a simple chain of poses to generate IMU measurements const double dt = 1.0; PoseRTV pose1, - pose2(Point3(1.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)), - pose3(Point3(2.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)), - pose4(Point3(3.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)); + pose2(Point3(1.0, 0.0, 0.0), Rot3::Ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)), + pose3(Point3(2.0, 0.0, 0.0), Rot3::Ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)), + pose4(Point3(3.0, 0.0, 0.0), Rot3::Ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)); // create measurements SharedDiagonal model = noiseModel::Isotropic::Sigma(9, 1.0); diff --git a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp index 0386d8bcd..db2f8f7f8 100644 --- a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp +++ b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp @@ -193,7 +193,7 @@ TEST( testPoseRTV, transformed_from_1 ) { Point3 T(1.0, 2.0, 3.0); Velocity3 V(2.0, 3.0, 4.0); PoseRTV start(R, T, V); - Pose3 transform(Rot3::yaw(M_PI_2), Point3(1.0, 2.0, 3.0)); + Pose3 transform(Rot3::Yaw(M_PI_2), Point3(1.0, 2.0, 3.0)); Matrix actDTrans, actDGlobal; PoseRTV actual = start.transformed_from(transform, actDGlobal, actDTrans); @@ -212,7 +212,7 @@ TEST( testPoseRTV, transformed_from_2 ) { Point3 T(1.0, 2.0, 3.0); Velocity3 V(2.0, 3.0, 4.0); PoseRTV start(R, T, V); - Pose3 transform(Rot3::yaw(M_PI_2), Point3(1.0, 2.0, 3.0)); + Pose3 transform(Rot3::Yaw(M_PI_2), Point3(1.0, 2.0, 3.0)); Matrix actDTrans, actDGlobal; PoseRTV actual = start.transformed_from(transform, actDGlobal, actDTrans); @@ -229,14 +229,14 @@ TEST( testPoseRTV, transformed_from_2 ) { TEST(testPoseRTV, RRTMbn) { EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(kZero3))); EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(Rot3()))); - EXPECT(assert_equal(PoseRTV::RRTMbn(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::ypr(0.1, 0.2, 0.3)))); + EXPECT(assert_equal(PoseRTV::RRTMbn(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::Ypr(0.1, 0.2, 0.3)))); } /* ************************************************************************* */ TEST(testPoseRTV, RRTMnb) { EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(kZero3))); EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(Rot3()))); - EXPECT(assert_equal(PoseRTV::RRTMnb(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::ypr(0.1, 0.2, 0.3)))); + EXPECT(assert_equal(PoseRTV::RRTMnb(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::Ypr(0.1, 0.2, 0.3)))); } /* ************************************************************************* */ diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp index a0d969c4d..1fb2cf39e 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -16,7 +16,7 @@ const double tol=1e-5; const double h = 0.01; //const double deg2rad = M_PI/180.0; -//Pose3 g1(Rot3::ypr(deg2rad*10.0, deg2rad*20.0, deg2rad*30.0), Point3(100.0, 200.0, 300.0)); +//Pose3 g1(Rot3::Ypr(deg2rad*10.0, deg2rad*20.0, deg2rad*30.0), Point3(100.0, 200.0, 300.0)); Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0)); //Vector6 v1((Vector(6) << 0.1, 0.05, 0.02, 10.0, 20.0, 30.0).finished()); Vector6 V1_w((Vector(6) << 0.0, 0.0, M_PI/3, 0.0, 0.0, 30.0).finished()); diff --git a/gtsam_unstable/geometry/Pose3Upright.cpp b/gtsam_unstable/geometry/Pose3Upright.cpp index 2ab3a6a9e..f83cb19fb 100644 --- a/gtsam_unstable/geometry/Pose3Upright.cpp +++ b/gtsam_unstable/geometry/Pose3Upright.cpp @@ -64,7 +64,7 @@ Rot2 Pose3Upright::rotation2() const { /* ************************************************************************* */ Rot3 Pose3Upright::rotation() const { - return Rot3::yaw(theta()); + return Rot3::Yaw(theta()); } /* ************************************************************************* */ diff --git a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp index 2f3c763dd..6ac3389ed 100644 --- a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp +++ b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp @@ -17,7 +17,7 @@ using namespace std; using namespace gtsam; static Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); -Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); SimpleCamera level_camera(level_pose, *K); /* ************************************************************************* */ @@ -142,7 +142,7 @@ TEST(InvDepthFactor, backproject2) // backwards facing camera Vector expected((Vector(5) << -5.,-5.,2., 3., -0.1).finished()); double inv_depth(1./10); - InvDepthCamera3 inv_camera(Pose3(Rot3::ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K); + InvDepthCamera3 inv_camera(Pose3(Rot3::Ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K); Point2 z = inv_camera.project(expected, inv_depth); Vector5 actual_vec; diff --git a/gtsam_unstable/geometry/tests/testPose3Upright.cpp b/gtsam_unstable/geometry/tests/testPose3Upright.cpp index 0929035b6..cd54a8813 100644 --- a/gtsam_unstable/geometry/tests/testPose3Upright.cpp +++ b/gtsam_unstable/geometry/tests/testPose3Upright.cpp @@ -58,9 +58,9 @@ TEST( testPose3Upright, conversions ) { EXPECT(assert_equal(Point3(1.0, 2.0, 3.0), pose.translation(), tol)); EXPECT(assert_equal(Point2(1.0, 2.0), pose.translation2(), tol)); EXPECT(assert_equal(Rot2::fromAngle(0.1), pose.rotation2(), tol)); - EXPECT(assert_equal(Rot3::yaw(0.1), pose.rotation(), tol)); + EXPECT(assert_equal(Rot3::Yaw(0.1), pose.rotation(), tol)); EXPECT(assert_equal(Pose2(1.0, 2.0, 0.1), pose.pose2(), tol)); - EXPECT(assert_equal(Pose3(Rot3::yaw(0.1), Point3(1.0, 2.0, 3.0)), pose.pose(), tol)); + EXPECT(assert_equal(Pose3(Rot3::Yaw(0.1), Point3(1.0, 2.0, 3.0)), pose.pose(), tol)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 14dfb8f1a..849206a7d 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -56,14 +56,14 @@ TEST(Similarity3, Getters) { //****************************************************************************** TEST(Similarity3, Getters2) { - Similarity3 test(Rot3::ypr(1, 2, 3), Point3(4, 5, 6), 7); - EXPECT(assert_equal(Rot3::ypr(1, 2, 3), test.rotation())); + Similarity3 test(Rot3::Ypr(1, 2, 3), Point3(4, 5, 6), 7); + EXPECT(assert_equal(Rot3::Ypr(1, 2, 3), test.rotation())); EXPECT(assert_equal(Point3(4, 5, 6), test.translation())); EXPECT_DOUBLES_EQUAL(7.0, test.scale(), 1e-9); } TEST(Similarity3, AdjointMap) { - Similarity3 test(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7); + Similarity3 test(Rot3::Ypr(1,2,3).inverse(), Point3(4,5,6), 7); Matrix7 result; result << -1.5739, -2.4512, -6.3651, -50.7671, -11.2503, 16.8859, -28.0000, 6.3167, -2.9884, -0.4111, 0.8502, 8.6373, -49.7260, -35.0000, @@ -76,7 +76,7 @@ TEST(Similarity3, AdjointMap) { } TEST(Similarity3, inverse) { - Similarity3 test(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7); + Similarity3 test(Rot3::Ypr(1,2,3).inverse(), Point3(4,5,6), 7); Matrix3 Re; Re << -0.2248, 0.9024, -0.3676, -0.3502, -0.4269, -0.8337, @@ -87,8 +87,8 @@ TEST(Similarity3, inverse) { } TEST(Similarity3, multiplication) { - Similarity3 test1(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7); - Similarity3 test2(Rot3::ypr(1,2,3).inverse(), Point3(8,9,10), 11); + Similarity3 test1(Rot3::Ypr(1,2,3).inverse(), Point3(4,5,6), 7); + Similarity3 test2(Rot3::Ypr(1,2,3).inverse(), Point3(8,9,10), 11); Matrix3 re; re << 0.0688, 0.9863, -0.1496, -0.5665, -0.0848, -0.8197, @@ -117,14 +117,14 @@ TEST(Similarity3, Manifold) { v3 << 0, 0, 0, 1, 2, 3, 0; EXPECT(assert_equal(v3, sim2.localCoordinates(sim3))); -// Similarity3 other = Similarity3(Rot3::ypr(0.01, 0.02, 0.03), Point3(0.4, 0.5, 0.6), 1); - Similarity3 other = Similarity3(Rot3::ypr(0.1, 0.2, 0.3),Point3(4,5,6),1); +// Similarity3 other = Similarity3(Rot3::Ypr(0.01, 0.02, 0.03), Point3(0.4, 0.5, 0.6), 1); + Similarity3 other = Similarity3(Rot3::Ypr(0.1, 0.2, 0.3),Point3(4,5,6),1); Vector vlocal = sim.localCoordinates(other); EXPECT(assert_equal(sim.retract(vlocal), other, 1e-2)); - Similarity3 other2 = Similarity3(Rot3::ypr(0.3, 0, 0),Point3(4,5,6),1); + Similarity3 other2 = Similarity3(Rot3::Ypr(0.3, 0, 0),Point3(4,5,6),1); Rot3 R = Rot3::Rodrigues(0.3,0,0); Vector vlocal2 = sim.localCoordinates(other2); @@ -167,7 +167,7 @@ TEST(Similarity3, manifold_first_order) } TEST(Similarity3, Optimization) { - Similarity3 prior = Similarity3(Rot3::ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4); + Similarity3 prior = Similarity3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4); noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1); Symbol key('x',1); PriorFactor factor(key, prior, model); @@ -187,10 +187,10 @@ TEST(Similarity3, Optimization) { TEST(Similarity3, Optimization2) { Similarity3 prior = Similarity3(); - Similarity3 m1 = Similarity3(Rot3::ypr(M_PI/4.0, 0, 0), Point3(2.0, 0, 0), 1.0); - Similarity3 m2 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(sqrt(8)*0.9, 0, 0), 1.0); - Similarity3 m3 = Similarity3(Rot3::ypr(3*M_PI/4.0, 0, 0), Point3(sqrt(32)*0.8, 0, 0), 1.0); - Similarity3 m4 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(6*0.7, 0, 0), 1.0); + Similarity3 m1 = Similarity3(Rot3::Ypr(M_PI/4.0, 0, 0), Point3(2.0, 0, 0), 1.0); + Similarity3 m2 = Similarity3(Rot3::Ypr(M_PI/2.0, 0, 0), Point3(sqrt(8)*0.9, 0, 0), 1.0); + Similarity3 m3 = Similarity3(Rot3::Ypr(3*M_PI/4.0, 0, 0), Point3(sqrt(32)*0.8, 0, 0), 1.0); + Similarity3 m4 = Similarity3(Rot3::Ypr(M_PI/2.0, 0, 0), Point3(6*0.7, 0, 0), 1.0); Similarity3 loop = Similarity3(1.42); //prior.print("Goal Transform"); @@ -220,10 +220,10 @@ TEST(Similarity3, Optimization2) { Values initial; initial.insert(X(1), Similarity3()); - initial.insert(X(2), Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(1, 0, 0), 1.1)); - initial.insert(X(3), Similarity3(Rot3::ypr(2.0*M_PI/2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2)); - initial.insert(X(4), Similarity3(Rot3::ypr(3.0*M_PI/2.0, 0, 0), Point3(0, 1, 0), 1.3)); - initial.insert(X(5), Similarity3(Rot3::ypr(4.0*M_PI/2.0, 0, 0), Point3(0, 0, 0), 1.0)); + initial.insert(X(2), Similarity3(Rot3::Ypr(M_PI/2.0, 0, 0), Point3(1, 0, 0), 1.1)); + initial.insert(X(3), Similarity3(Rot3::Ypr(2.0*M_PI/2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2)); + initial.insert(X(4), Similarity3(Rot3::Ypr(3.0*M_PI/2.0, 0, 0), Point3(0, 1, 0), 1.3)); + initial.insert(X(5), Similarity3(Rot3::Ypr(4.0*M_PI/2.0, 0, 0), Point3(0, 0, 0), 1.0)); //initial.print("Initial Estimate\n"); diff --git a/gtsam_unstable/slam/Mechanization_bRn2.cpp b/gtsam_unstable/slam/Mechanization_bRn2.cpp index f6f1c4a5c..20ffbdd4f 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.cpp +++ b/gtsam_unstable/slam/Mechanization_bRn2.cpp @@ -60,7 +60,7 @@ Mechanization_bRn2 Mechanization_bRn2::initialize(const Matrix& U, double pitch = atan2(-g1, sqrt(g2 * g2 + g3 * g3)); double yaw = 0; // This returns body-to-nav nRb - Rot3 bRn = Rot3::ypr(yaw, pitch, roll).inverse(); + Rot3 bRn = Rot3::Ypr(yaw, pitch, roll).inverse(); return Mechanization_bRn2(bRn, x_g, x_a); } diff --git a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp index 407c9a345..99f494ad9 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp @@ -22,7 +22,7 @@ static Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); static SharedNoiseModel sigma(noiseModel::Unit::Create(2)); // camera pose at (0,0,1) looking straight along the x-axis. -Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); SimpleCamera level_camera(level_pose, *K); typedef InvDepthFactor3 InverseDepthFactor; diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp index 99a4f90a4..3aa758701 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp @@ -24,8 +24,8 @@ using namespace gtsam; TEST( InvDepthFactorVariant1, optimize) { // Create two poses looking in the x-direction - Pose3 pose1(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1.0)); - Pose3 pose2(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1.5)); + Pose3 pose1(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1.0)); + Pose3 pose2(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1.5)); // Create a landmark 5 meters in front of pose1 (camera center at (0,0,1)) Point3 landmark(5, 0, 1); diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp index 2c85b3dad..d20fc000c 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp @@ -24,8 +24,8 @@ using namespace gtsam; TEST( InvDepthFactorVariant2, optimize) { // Create two poses looking in the x-direction - Pose3 pose1(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.0)); - Pose3 pose2(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.5)); + Pose3 pose1(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.0)); + Pose3 pose2(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.5)); // Create a landmark 5 meters in front of pose1 (camera center at (0,0,1)) Point3 landmark(5, 0, 1); diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp index ec9aa90c3..a6eed54d5 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp @@ -24,8 +24,8 @@ using namespace gtsam; TEST( InvDepthFactorVariant3, optimize) { // Create two poses looking in the x-direction - Pose3 pose1(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.0)); - Pose3 pose2(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.5)); + Pose3 pose1(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.0)); + Pose3 pose2(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.5)); // Create a landmark 5 meters in front of pose1 (camera center at (0,0,1)) Point3 landmark(5, 0, 1); diff --git a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp index c6ad9a38b..a06c39182 100644 --- a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp +++ b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp @@ -18,7 +18,7 @@ SharedNoiseModel model1 = noiseModel::Unit::Create(1); const double tol = 1e-5; const Pose3 pose1(Rot3(), Point3(2.0, 3.0, 4.0)); -const Pose3 pose2(Rot3::pitch(-M_PI_2), Point3(2.0, 3.0, 4.0)); +const Pose3 pose2(Rot3::Pitch(-M_PI_2), Point3(2.0, 3.0, 4.0)); const Pose3 pose3(Rot3::RzRyRx(0.1, 0.2, 0.3), Point3(2.0, 3.0, 4.0)); const Point3 point1(3.0, 4.0, 2.0); const gtsam::Key poseKey = 1, pointKey = 2; diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 2981f675d..5bad0e171 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -115,7 +115,7 @@ TEST( SmartStereoProjectionPoseFactor, Equals ) { /* *************************************************************************/ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), + Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera level_camera(level_pose, K2); @@ -154,7 +154,7 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) { /* *************************************************************************/ TEST( SmartStereoProjectionPoseFactor, noisy ) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), + Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera level_camera(level_pose, K2); @@ -172,7 +172,7 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) { Values values; values.insert(x1, level_pose); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); values.insert(x2, level_pose_right.compose(noise_pose)); @@ -206,7 +206,7 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) { TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K2); // create second camera 1 meter to the right of first camera @@ -257,8 +257,8 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); @@ -354,7 +354,7 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K); // create second camera 1 meter to the right of first camera Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); @@ -397,8 +397,8 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); @@ -422,7 +422,7 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K); // create second camera 1 meter to the right of first camera Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); @@ -466,8 +466,8 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); @@ -490,7 +490,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K); // create second camera 1 meter to the right of first camera Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); @@ -548,7 +548,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); @@ -587,7 +587,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { // views.push_back(x3); // // // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) -// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K); // // create second camera 1 meter to the right of first camera // Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); @@ -626,8 +626,8 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { // graph.push_back(PriorFactor(x1, pose1, noisePrior)); // graph.push_back(PriorFactor(x2, pose2, noisePrior)); // -// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), Point3(0.1,0.1,0.1)); // smaller noise +// // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below +// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/100, 0., -M_PI/100), Point3(0.1,0.1,0.1)); // smaller noise // Values values; // values.insert(x1, pose1); // values.insert(x2, pose2); @@ -648,7 +648,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { // views.push_back(x3); // // // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) -// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K2); // // // create second camera 1 meter to the right of first camera @@ -684,7 +684,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { // graph.push_back(PriorFactor(x1, pose1, noisePrior)); // graph.push_back(PriorFactor(x2, pose2, noisePrior)); // -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); +// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // Values values; // values.insert(x1, pose1); // values.insert(x2, pose2); @@ -708,7 +708,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K); // create second camera @@ -754,7 +754,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { values.insert(x1, pose1); values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise values.insert(x3, pose3 * noise_pose); @@ -796,7 +796,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // views.push_back(x3); // // // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) -// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K2); // // // create second camera 1 meter to the right of first camera @@ -835,7 +835,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); // graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); // -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.1,0.1,0.1)); // smaller noise +// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.1,0.1,0.1)); // smaller noise // Values values; // values.insert(x1, pose1); // values.insert(x2, pose2*noise_pose); @@ -862,7 +862,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // views.push_back(x3); // // // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) -// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K); // // // create second camera 1 meter to the right of first camera @@ -908,8 +908,8 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); // graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); // -// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), Point3(0.1,0.1,0.1)); // smaller noise +// // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below +// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/100, 0., -M_PI/100), Point3(0.1,0.1,0.1)); // smaller noise // Values values; // values.insert(x1, pose1); // values.insert(x2, pose2); @@ -935,7 +935,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // views.push_back(x2); // // // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) -// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K2); // // // create second camera 1 meter to the right of first camera @@ -955,7 +955,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor()); // smartFactor1->add(measurements_cam1,views, model, K2); // -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); +// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // Values values; // values.insert(x1, pose1); // values.insert(x2, pose2); @@ -977,7 +977,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K); // create second camera 1 meter to the right of first camera @@ -1005,7 +1005,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { smartFactorInstance->linearize(values); // hessianFactor->print("Hessian factor \n"); - Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); + Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; rotValues.insert(x1, poseDrift.compose(pose1)); @@ -1021,7 +1021,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-7)); - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), + Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), Point3(10, -4, 5)); Values tranValues; @@ -1047,7 +1047,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K2); // Second and third cameras in same place, which is a degenerate configuration @@ -1072,7 +1072,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { boost::shared_ptr hessianFactor = smartFactor->linearize( values); - Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); + Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; rotValues.insert(x1, poseDrift.compose(pose1)); @@ -1087,7 +1087,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-6)); - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), + Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), Point3(10, -4, 5)); Values tranValues; From 6eece9cc603cf20ad9a40a777819ba5d172f3940 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 26 Jan 2016 23:57:34 -0800 Subject: [PATCH 3/6] Quaternion now also uppercase --- gtsam/geometry/Rot3.cpp | 2 +- gtsam/geometry/Rot3.h | 21 +++++++++------- gtsam/geometry/Rot3M.cpp | 6 ++--- gtsam/geometry/Rot3Q.cpp | 18 ++++++------- gtsam/geometry/tests/testRot3.cpp | 4 +-- gtsam/nonlinear/tests/testAdaptAutoDiff.cpp | 2 +- gtsam/slam/dataset.cpp | 4 +-- gtsam/slam/tests/testDataset.cpp | 28 ++++++++++----------- python/handwritten/geometry/Rot3.cpp | 6 ++--- 9 files changed, 47 insertions(+), 44 deletions(-) diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 6b28f5125..ef384c3ef 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -123,7 +123,7 @@ Vector3 Rot3::rpy() const { /* ************************************************************************* */ Vector Rot3::quaternion() const { - Quaternion q = toQuaternion(); + gtsam::Quaternion q = toQuaternion(); Vector v(4); v(0) = q.w(); v(1) = q.x(); diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 4dd582a77..5b7acb4be 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -59,7 +59,7 @@ namespace gtsam { #ifdef GTSAM_USE_QUATERNIONS /** Internal Eigen Quaternion */ - Quaternion quaternion_; + gtsam::Quaternion quaternion_; #else Matrix3 rot_; #endif @@ -166,8 +166,8 @@ namespace gtsam { static Rot3 Ypr(double y, double p, double r) { return RzRyRx(r,p,y);} /** Create from Quaternion coefficients */ - static Rot3 quaternion(double w, double x, double y, double z) { - Quaternion q(w, x, y, z); + static Rot3 Quaternion(double w, double x, double y, double z) { + gtsam::Quaternion q(w, x, y, z); return Rot3(q); } @@ -179,7 +179,7 @@ namespace gtsam { */ static Rot3 AxisAngle(const Vector3& axis, double angle) { #ifdef GTSAM_USE_QUATERNIONS - return Quaternion(Eigen::AngleAxis(angle, axis)); + return gtsam::Quaternion(Eigen::AngleAxis(angle, axis)); #else return SO3::AxisAngle(axis,angle); #endif @@ -313,7 +313,7 @@ namespace gtsam { static Rot3 Expmap(const Vector3& v, OptionalJacobian<3,3> H = boost::none) { if(H) *H = Rot3::ExpmapDerivative(v); #ifdef GTSAM_USE_QUATERNIONS - return traits::Expmap(v); + return traits::Expmap(v); #else return traits::Expmap(v); #endif @@ -460,7 +460,7 @@ namespace gtsam { /** Compute the quaternion representation of this rotation. * @return The quaternion */ - Quaternion toQuaternion() const; + gtsam::Quaternion toQuaternion() const; /** * Converts to a generic matrix to allow for use with matlab @@ -479,6 +479,8 @@ namespace gtsam { GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p); /// @} + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @name Deprecated /// @{ static Rot3 rodriguez(const Vector3& axis, double angle) { return AxisAngle(axis, angle); } @@ -486,13 +488,14 @@ namespace gtsam { static Rot3 rodriguez(const Unit3& axis, double angle) { return AxisAngle(axis, angle); } static Rot3 rodriguez(const Vector3& w) { return Rodrigues(w); } static Rot3 rodriguez(double wx, double wy, double wz) { return Rodrigues(wx, wy, wz); } - /// @} - -#ifdef ALLOW_DEPRECATED_IN_GTSAM4 static Rot3 yaw (double t) { return Yaw(t); } static Rot3 pitch(double t) { return Pitch(t); } static Rot3 roll (double t) { return Roll(t); } static Rot3 ypr(double y, double p, double r) { return Ypr(r,p,y);} + static Rot3 quaternion(double w, double x, double y, double z) { + return Rot3::Quaternion q(w, x, y, z); + } + /// @} #endif private: diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index a7b654070..c3636000b 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -51,7 +51,7 @@ Rot3::Rot3(double R11, double R12, double R13, } /* ************************************************************************* */ -Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) { +Rot3::Rot3(const gtsam::Quaternion& q) : rot_(q.toRotationMatrix()) { } /* ************************************************************************* */ @@ -191,8 +191,8 @@ Point3 Rot3::r2() const { return Point3(rot_.col(1)); } Point3 Rot3::r3() const { return Point3(rot_.col(2)); } /* ************************************************************************* */ -Quaternion Rot3::toQuaternion() const { - return Quaternion(rot_); +gtsam::Quaternion Rot3::toQuaternion() const { + return gtsam::Quaternion(rot_); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 2497f6806..f8a01141b 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -47,30 +47,30 @@ namespace gtsam { R31, R32, R33).finished()) {} /* ************************************************************************* */ - Rot3::Rot3(const Quaternion& q) : + Rot3::Rot3(const gtsam::Quaternion& q) : quaternion_(q) { } /* ************************************************************************* */ Rot3 Rot3::Rx(double t) { - return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX())); + return gtsam::Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX())); } /* ************************************************************************* */ Rot3 Rot3::Ry(double t) { - return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitY())); + return gtsam::Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitY())); } /* ************************************************************************* */ Rot3 Rot3::Rz(double t) { - return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitZ())); + return gtsam::Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitZ())); } /* ************************************************************************* */ Rot3 Rot3::RzRyRx(double x, double y, double z) { return Rot3( - Quaternion(Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ())) * - Quaternion(Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY())) * - Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX()))); + gtsam::Quaternion(Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ())) * + gtsam::Quaternion(Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY())) * + gtsam::Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX()))); } /* ************************************************************************* */ @@ -98,7 +98,7 @@ namespace gtsam { /* ************************************************************************* */ Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3, 3> H) { - return traits::Logmap(R.quaternion_, H); + return traits::Logmap(R.quaternion_, H); } /* ************************************************************************* */ @@ -128,7 +128,7 @@ namespace gtsam { Point3 Rot3::r3() const { return Point3(quaternion_.toRotationMatrix().col(2)); } /* ************************************************************************* */ - Quaternion Rot3::toQuaternion() const { return quaternion_; } + gtsam::Quaternion Rot3::toQuaternion() const { return quaternion_; } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 296bd2b7c..3cf3f9387 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -594,9 +594,9 @@ TEST(Rot3, quaternion) { // Check creating Rot3 from quaternion EXPECT(assert_equal(R1, Rot3(q1))); - EXPECT(assert_equal(R1, Rot3::quaternion(q1.w(), q1.x(), q1.y(), q1.z()))); + EXPECT(assert_equal(R1, Rot3::Quaternion(q1.w(), q1.x(), q1.y(), q1.z()))); EXPECT(assert_equal(R2, Rot3(q2))); - EXPECT(assert_equal(R2, Rot3::quaternion(q2.w(), q2.x(), q2.y(), q2.z()))); + EXPECT(assert_equal(R2, Rot3::Quaternion(q2.w(), q2.x(), q2.y(), q2.z()))); // Check converting Rot3 to quaterion EXPECT(assert_equal(Vector(R1.toQuaternion().coeffs()), Vector(q1.coeffs()))); diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index 377d6cd34..16117845e 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -62,7 +62,7 @@ using namespace gtsam; // Check that ceres rotation convention is the same TEST(AdaptAutoDiff, Rotation) { Vector3 axisAngle(0.1, 0.2, 0.3); - Matrix3 expected = Rot3::rodriguez(axisAngle).matrix(); + Matrix3 expected = Rot3::Rodrigues(axisAngle).matrix(); Matrix3 actual; ceres::AngleAxisToRotationMatrix(axisAngle.data(), actual.data()); EXPECT(assert_equal(expected, actual)); diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 9e71e3753..50516afe4 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -518,7 +518,7 @@ GraphAndValues load3D(const string& filename) { Key id; double x, y, z, qx, qy, qz, qw; ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw; - Rot3 R = Rot3::quaternion(qw, qx, qy, qz); + Rot3 R = Rot3::Quaternion(qw, qx, qy, qz); Point3 t = Point3(x, y, z); initial->insert(id, Pose3(R,t)); } @@ -553,7 +553,7 @@ GraphAndValues load3D(const string& filename) { Key id1, id2; double x, y, z, qx, qy, qz, qw; ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw; - Rot3 R = Rot3::quaternion(qw, qx, qy, qz); + Rot3 R = Rot3::Quaternion(qw, qx, qy, qz); Point3 t = Point3(x, y, z); for (int i = 0; i < 6; i++){ for (int j = i; j < 6; j++){ diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 26a00360e..4e8e3dbf3 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -137,23 +137,23 @@ TEST( dataSet, readG2o3D) boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D); Values expectedValues; - Rot3 R0 = Rot3::quaternion(1.000000, 0.000000, 0.000000, 0.000000 ); + Rot3 R0 = Rot3::Quaternion(1.000000, 0.000000, 0.000000, 0.000000 ); Point3 p0 = Point3(0.000000, 0.000000, 0.000000); expectedValues.insert(0, Pose3(R0, p0)); - Rot3 R1 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); + Rot3 R1 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); Point3 p1 = Point3(1.001367, 0.015390, 0.004948); expectedValues.insert(1, Pose3(R1, p1)); - Rot3 R2 = Rot3::quaternion(0.421446, -0.351729, -0.597838, 0.584174 ); + Rot3 R2 = Rot3::Quaternion(0.421446, -0.351729, -0.597838, 0.584174 ); Point3 p2 = Point3(1.993500, 0.023275, 0.003793); expectedValues.insert(2, Pose3(R2, p2)); - Rot3 R3 = Rot3::quaternion(0.067024, 0.331798, -0.200659, 0.919323); + Rot3 R3 = Rot3::Quaternion(0.067024, 0.331798, -0.200659, 0.919323); Point3 p3 = Point3(2.004291, 1.024305, 0.018047); expectedValues.insert(3, Pose3(R3, p3)); - Rot3 R4 = Rot3::quaternion(0.765488, -0.035697, -0.462490, 0.445933); + Rot3 R4 = Rot3::Quaternion(0.765488, -0.035697, -0.462490, 0.445933); Point3 p4 = Point3(0.999908, 1.055073, 0.020212); expectedValues.insert(4, Pose3(R4, p4)); @@ -163,27 +163,27 @@ TEST( dataSet, readG2o3D) NonlinearFactorGraph expectedGraph; Point3 p01 = Point3(1.001367, 0.015390, 0.004948); - Rot3 R01 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); + Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); expectedGraph.add(BetweenFactor(0, 1, Pose3(R01,p01), model)); Point3 p12 = Point3(0.523923, 0.776654, 0.326659); - Rot3 R12 = Rot3::quaternion(0.105373 , 0.311512, 0.656877, -0.678505 ); + Rot3 R12 = Rot3::Quaternion(0.105373 , 0.311512, 0.656877, -0.678505 ); expectedGraph.add(BetweenFactor(1, 2, Pose3(R12,p12), model)); Point3 p23 = Point3(0.910927, 0.055169, -0.411761); - Rot3 R23 = Rot3::quaternion(0.568551 , 0.595795, -0.561677, 0.079353 ); + Rot3 R23 = Rot3::Quaternion(0.568551 , 0.595795, -0.561677, 0.079353 ); expectedGraph.add(BetweenFactor(2, 3, Pose3(R23,p23), model)); Point3 p34 = Point3(0.775288, 0.228798, -0.596923); - Rot3 R34 = Rot3::quaternion(0.542221 , -0.592077, 0.303380, -0.513226 ); + Rot3 R34 = Rot3::Quaternion(0.542221 , -0.592077, 0.303380, -0.513226 ); expectedGraph.add(BetweenFactor(3, 4, Pose3(R34,p34), model)); Point3 p14 = Point3(-0.577841, 0.628016, -0.543592); - Rot3 R14 = Rot3::quaternion(0.327419 , -0.125250, -0.534379, 0.769122 ); + Rot3 R14 = Rot3::Quaternion(0.327419 , -0.125250, -0.534379, 0.769122 ); expectedGraph.add(BetweenFactor(1, 4, Pose3(R14,p14), model)); Point3 p30 = Point3(-0.623267, 0.086928, 0.773222); - Rot3 R30 = Rot3::quaternion(0.083672 , 0.104639, 0.627755, 0.766795 ); + Rot3 R30 = Rot3::Quaternion(0.083672 , 0.104639, 0.627755, 0.766795 ); expectedGraph.add(BetweenFactor(3, 0, Pose3(R30,p30), model)); EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); @@ -199,11 +199,11 @@ TEST( dataSet, readG2o3DNonDiagonalNoise) boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D); Values expectedValues; - Rot3 R0 = Rot3::quaternion(1.000000, 0.000000, 0.000000, 0.000000 ); + Rot3 R0 = Rot3::Quaternion(1.000000, 0.000000, 0.000000, 0.000000 ); Point3 p0 = Point3(0.000000, 0.000000, 0.000000); expectedValues.insert(0, Pose3(R0, p0)); - Rot3 R1 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); + Rot3 R1 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); Point3 p1 = Point3(1.001367, 0.015390, 0.004948); expectedValues.insert(1, Pose3(R1, p1)); @@ -223,7 +223,7 @@ TEST( dataSet, readG2o3DNonDiagonalNoise) noiseModel::Gaussian::shared_ptr model = noiseModel::Gaussian::Covariance(Info.inverse()); NonlinearFactorGraph expectedGraph; Point3 p01 = Point3(1.001367, 0.015390, 0.004948); - Rot3 R01 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); + Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); expectedGraph.add(BetweenFactor(0, 1, Pose3(R01,p01), model)); EXPECT(assert_equal(expectedGraph,*actualGraph,1e-2)); diff --git a/python/handwritten/geometry/Rot3.cpp b/python/handwritten/geometry/Rot3.cpp index b4551ca5c..68643fe2c 100644 --- a/python/handwritten/geometry/Rot3.cpp +++ b/python/handwritten/geometry/Rot3.cpp @@ -27,12 +27,12 @@ using namespace gtsam; static Rot3 Quaternion_0(const Vector4& q) { - return Rot3::quaternion(q[0],q[1],q[2],q[3]); + return Rot3::Quaternion(q[0],q[1],q[2],q[3]); } static Rot3 Quaternion_1(double w, double x, double y, double z) { - return Rot3::quaternion(w,x,y,z); + return Rot3::Quaternion(w,x,y,z); } // Prototypes used to perform overloading @@ -108,4 +108,4 @@ void exportRot3(){ .def(repr(self)) // __repr__ ; -} \ No newline at end of file +} From f078741ed402322393870ab6c9a82ff9fa7658f1 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 26 Jan 2016 23:57:44 -0800 Subject: [PATCH 4/6] New GTSAM option --- CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index e8da98ffd..71f2dd939 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -65,6 +65,7 @@ option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" ON) option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON) option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module" ON) +option(GTSAM_ALLOW_DEPRECATED_SINCE_V4 "Allow use of methods/functions deprecated in GTSAM 4" ON) # Options relating to MATLAB wrapper # TODO: Check for matlab mex binary before handling building of binaries @@ -471,6 +472,7 @@ print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default R print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ") print_config_flag(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ") print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ") +print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4} "Deprecated in GTSAM 4 allowed ") message(STATUS "MATLAB toolbox flags ") print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ") From 88fad4caffeceeccd13455a1aab62ad1e4db6aad Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 27 Jan 2016 09:24:49 -0800 Subject: [PATCH 5/6] Fix small bugs with MATLAB wrapping --- gtsam.h | 10 +++++----- gtsam/geometry/triangulation.h | 1 + gtsam/sam/RangeFactor.h | 3 +-- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/gtsam.h b/gtsam.h index ca014ad5d..57a1e09be 100644 --- a/gtsam.h +++ b/gtsam.h @@ -434,11 +434,11 @@ class Rot3 { 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 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 diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index bec901830..c6e613b14 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -19,6 +19,7 @@ #pragma once #include +#include #include #include #include diff --git a/gtsam/sam/RangeFactor.h b/gtsam/sam/RangeFactor.h index 1ad27865d..a5bcac822 100644 --- a/gtsam/sam/RangeFactor.h +++ b/gtsam/sam/RangeFactor.h @@ -31,8 +31,7 @@ struct Range; * Works for any two types A1,A2 for which the functor Range() is defined * @addtogroup SAM */ -template ::result_type> +template class RangeFactor : public ExpressionFactor2 { private: typedef RangeFactor This; From 540772819b2e58189c1d00db1fd024c31952cbe9 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 27 Jan 2016 22:07:46 -0800 Subject: [PATCH 6/6] Added definition --- CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 71f2dd939..9d543b87b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -318,6 +318,10 @@ if(GTSAM_ENABLE_CONSISTENCY_CHECKS) add_definitions(-DGTSAM_EXTRA_CONSISTENCY_CHECKS) endif() +if(GTSAM_ENABLE_CONSISTENCY_CHECKS) + add_definitions(-DGTSAM_ALLOW_DEPRECATED_SINCE_V4) +endif() + ############################################################################### # Add components