diff --git a/gtsam.h b/gtsam.h index b270a17ec..432c95698 100644 --- a/gtsam.h +++ b/gtsam.h @@ -26,8 +26,10 @@ /** * Status: - * - Depreciated versions of functions that return a shared_ptr unnecessarily are still present * - TODO: global functions + * - TODO: default values for arguments + * - TODO: overloaded functions + * - TODO: Handle Rot3M conversions to quaternions * - TODO: namespace detection to handle nested namespaces */ @@ -43,12 +45,6 @@ class Point2 { Point2 compose(const Point2& p2); Point2 between(const Point2& p2); Point2 retract(Vector v); - - // Depreciated interface - Point2* compose_(const Point2& p2); - Point2* between_(const Point2& p2); - Point2* retract_(Vector v); - static Point2* Expmap_(Vector v); }; class Point3 { @@ -67,12 +63,6 @@ class Point3 { Point3 retract(Vector v); Point3 compose(const Point3& p2); Point3 between(const Point3& p2); - - // Depreciated interface - static Point3* Expmap_(Vector v); - Point3* compose_(const Point3& p2); - Point3* between_(const Point3& p2); - Point3* retract_(Vector v); }; class Rot2 { @@ -80,20 +70,21 @@ class Rot2 { Rot2(double theta); static Rot2 Expmap(Vector v); static Vector Logmap(const Rot2& p); + static Rot2 fromAngle(double theta); + static Rot2 fromDegrees(double theta); + static Rot2 fromCosSin(double c, double s); + static Rot2 relativeBearing(const Point2& d); // Ignoring derivative + static Rot2 atan2(double y, double x); void print(string s) const; bool equals(const Rot2& rot, double tol) const; + double theta() const; + double degrees() const; double c() const; double s() const; Vector localCoordinates(const Rot2& p); Rot2 retract(Vector v); Rot2 compose(const Rot2& p2); Rot2 between(const Rot2& p2); - - // Depreciated interface - Rot2* compose_(const Rot2& p2); - Rot2* between_(const Rot2& p2); - Rot2* retract_(Vector v); - static Rot2* Expmap_(Vector v); }; class Rot3 { @@ -102,22 +93,27 @@ class Rot3 { static Rot3 Expmap(Vector v); static Vector Logmap(const Rot3& p); static Rot3 ypr(double y, double p, double r); + static Rot3 Rx(double t); + static Rot3 Ry(double t); + static Rot3 Rz(double t); + static Rot3 RzRyRx(double x, double y, double z); + static Rot3 RzRyRx(const Vector& xyz); + static Rot3 yaw (double t); // positive yaw is to right (as in aircraft heading) + static Rot3 pitch(double t); // positive pitch is up (increasing aircraft altitude) + static Rot3 roll (double t); // positive roll is to right (increasing yaw in aircraft) + static Rot3 quaternion(double w, double x, double y, double z); + static Rot3 rodriguez(const Vector& v); Matrix matrix() const; Matrix transpose() const; Vector xyz() const; Vector ypr() const; +// Vector toQuaternion() const; // FIXME: Can't cast to Vector properly void print(string s) const; bool equals(const Rot3& rot, double tol) const; Vector localCoordinates(const Rot3& p); Rot3 retract(Vector v); Rot3 compose(const Rot3& p2); Rot3 between(const Rot3& p2); - - // Depreciated interface - Rot3* compose_(const Rot3& p2); - Rot3* between_(const Rot3& p2); - Rot3* retract_(Vector v); - static Rot3* Expmap_(Vector v); }; class Pose2 { @@ -138,12 +134,6 @@ class Pose2 { Pose2 retract(Vector v); Pose2 compose(const Pose2& p2); Pose2 between(const Pose2& p2); - - // Depreciated interface - Pose2* compose_(const Pose2& p2); - Pose2* between_(const Pose2& p2); - Pose2* retract_(Vector v); - static Pose2* Expmap_(Vector v); }; class Pose3 { @@ -165,14 +155,6 @@ class Pose3 { Pose3 retract(Vector v); Point3 translation() const; Rot3 rotation() const; - - // Depreciated interface - static Pose3* Expmap_(Vector v); - Pose3* compose_(const Pose3& p2); - Pose3* between_(const Pose3& p2); - Pose3* retract_(Vector v); - Point3* translation_() const; - Rot3* rotation_() const; }; class SharedGaussian { @@ -314,7 +296,7 @@ class PlanarSLAMOdometry { class GaussianSequentialSolver { GaussianSequentialSolver(const GaussianFactorGraph& graph, bool useQR); GaussianBayesNet* eliminate() const; - VectorValues* optimize() const; // FAIL: parse error here + VectorValues* optimize() const; GaussianFactor* marginalFactor(int j) const; Matrix marginalCovariance(int j) const; }; diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 18a390d7b..12f299637 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -72,11 +72,6 @@ public: return *this + p2; } - /// MATLAB version returns shared pointer - boost::shared_ptr compose_(const Point2& p2) { - return boost::shared_ptr(new Point2(compose(p2))); - } - /** operators */ inline Point2 operator- () const {return Point2(-x_,-y_);} inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);} @@ -97,11 +92,6 @@ public: /// Updates a with tangent space delta inline Point2 retract(const Vector& v) const { return *this + Point2(v); } - /// MATLAB version returns shared pointer - boost::shared_ptr retract_(const Vector& v) { - return boost::shared_ptr(new Point2(retract(v))); - } - /// Local coordinates of manifold neighborhood around current value inline Vector localCoordinates(const Point2& t2) const { return Logmap(between(t2)); } @@ -111,9 +101,6 @@ public: /// Exponential map around identity - just create a Point2 from a vector static inline Point2 Expmap(const Vector& v) { return Point2(v); } - static inline boost::shared_ptr Expmap_(const Vector& v) { - return boost::shared_ptr(new Point2(Expmap(v))); - } /// Log map around identity - just return the Point2 as a vector static inline Vector Logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); } @@ -149,11 +136,6 @@ public: return p2 - (*this); } - /// MATLAB version returns shared pointer - boost::shared_ptr between_(const Point2& p2) { - return boost::shared_ptr(new Point2(between(p2))); - } - /** get functions for x, y */ double x() const {return x_;} double y() const {return y_;} diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 7b1520a16..a01f5009b 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -76,11 +76,6 @@ namespace gtsam { return *this + p2; } - /// MATLAB version returns shared pointer - boost::shared_ptr compose_(const Point3& p2) { - return boost::shared_ptr(new Point3(compose(p2))); - } - /// @} /// @name Manifold /// @{ @@ -94,11 +89,6 @@ namespace gtsam { /// Updates a with tangent space delta inline Point3 retract(const Vector& v) const { return compose(Expmap(v)); } - /// MATLAB version returns shared pointer - boost::shared_ptr retract_(const Vector& v) { - return boost::shared_ptr(new Point3(retract(v))); - } - /// Returns inverse retraction inline Vector localCoordinates(const Point3& t2) const { return Logmap(t2) - Logmap(*this); } @@ -108,9 +98,6 @@ namespace gtsam { /** Exponential map at identity - just create a Point3 from x,y,z */ static inline Point3 Expmap(const Vector& v) { return Point3(v); } - static inline boost::shared_ptr Expmap_(const Vector& v) { - return boost::shared_ptr(new Point3(Expmap(v))); - } /** Log map at identity - return the x,y,z of this point */ static inline Vector Logmap(const Point3& dp) { return Vector_(3, dp.x(), dp.y(), dp.z()); } @@ -151,11 +138,6 @@ namespace gtsam { return p2 - *this; } - /// MATLAB version returns shared pointer - boost::shared_ptr between_(const Point3& p2) { - return boost::shared_ptr(new Point3(between(p2))); - } - /** return vectorized form (column-wise)*/ Vector vector() const { Vector v(3); v(0)=x_; v(1)=y_; v(2)=z_; diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index cee792ea6..74cb2453d 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -103,11 +103,6 @@ public: boost::optional H1 = boost::none, boost::optional H2 = boost::none) const; - /// MATLAB version returns shared pointer - boost::shared_ptr compose_(const Pose2& p2) { - return boost::shared_ptr(new Pose2(compose(p2))); - } - /// compose syntactic sugar inline Pose2 operator*(const Pose2& p2) const { return Pose2(r_*p2.r(), t_ + r_*p2.t()); @@ -126,11 +121,6 @@ public: /// Retraction from R^3 to Pose2 manifold neighborhood around current pose Pose2 retract(const Vector& v) const; - /// MATLAB version returns shared pointer - boost::shared_ptr retract_(const Vector& v) { - return boost::shared_ptr(new Pose2(retract(v))); - } - /// Local 3D coordinates of Pose2 manifold neighborhood around current pose Vector localCoordinates(const Pose2& p2) const; @@ -140,9 +130,6 @@ public: /// Exponential map from Lie algebra se(2) to SE(2) static Pose2 Expmap(const Vector& xi); - static inline boost::shared_ptr Expmap_(const Vector& v) { - return boost::shared_ptr(new Pose2(Expmap(v))); - } /// Exponential map from SE(2) to Lie algebra se(2) static Vector Logmap(const Pose2& p); @@ -159,11 +146,6 @@ public: boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /// MATLAB version returns shared pointer - boost::shared_ptr between_(const Pose2& p2) { - return boost::shared_ptr(new Pose2(between(p2))); - } - /** * Return point coordinates in pose coordinate frame */ diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index be9966c60..3bcda0c00 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -96,11 +96,6 @@ namespace gtsam { boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /// MATLAB version returns shared pointer - boost::shared_ptr compose_(const Pose3& p2) { - return boost::shared_ptr(new Pose3(compose(p2))); - } - /// compose syntactic sugar Pose3 operator*(const Pose3& T) const { return Pose3(R_*T.R_, t_ + R_*T.t_); @@ -120,11 +115,6 @@ namespace gtsam { /// Retraction from R^6 to Pose3 manifold neighborhood around current pose Pose3 retract(const Vector& d) const; - /// MATLAB version returns shared pointer - boost::shared_ptr retract_(const Vector& v) { - return boost::shared_ptr(new Pose3(retract(v))); - } - /// Local 6D coordinates of Pose3 manifold neighborhood around current pose Vector localCoordinates(const Pose3& T2) const; @@ -134,9 +124,6 @@ namespace gtsam { /// Exponential map from Lie algebra se(3) to SE(3) static Pose3 Expmap(const Vector& xi); - static inline boost::shared_ptr Expmap_(const Vector& v) { - return boost::shared_ptr(new Pose3(Expmap(v))); - } /// Exponential map from SE(3) to Lie algebra se(3) static Vector Logmap(const Pose3& p); @@ -164,11 +151,6 @@ namespace gtsam { boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /// MATLAB version returns shared pointer - boost::shared_ptr between_(const Pose3& p2) { - return boost::shared_ptr(new Pose3(between(p2))); - } - /** * Calculate Adjoint map * Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index c52776cb9..1a6d91b20 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -138,11 +138,6 @@ namespace gtsam { return *this * R1; } - /// MATLAB version returns shared pointer - boost::shared_ptr compose_(const Rot2& p2) { - return boost::shared_ptr(new Rot2(compose(p2))); - } - /** Compose - make a new rotation by adding angles */ Rot2 operator*(const Rot2& R) const { return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_); @@ -170,11 +165,6 @@ namespace gtsam { /// Updates a with tangent space delta inline Rot2 retract(const Vector& v) const { return *this * Expmap(v); } - /// MATLAB version returns shared pointer - boost::shared_ptr retract_(const Vector& v) { - return boost::shared_ptr(new Rot2(retract(v))); - } - /// Returns inverse retraction inline Vector localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); } @@ -189,9 +179,6 @@ namespace gtsam { else return Rot2::fromAngle(v(0)); } - static inline boost::shared_ptr Expmap_(const Vector& v) { - return boost::shared_ptr(new Rot2(Expmap(v))); - } /// Logmap around identity - return the angle of the rotation static inline Vector Logmap(const Rot2& r) { @@ -219,11 +206,6 @@ namespace gtsam { return between_default(*this, p2); } - /// MATLAB version returns shared pointer - boost::shared_ptr between_(const Rot2& p2) { - return boost::shared_ptr(new Rot2(between(p2))); - } - /** return 2*2 rotation matrix */ Matrix matrix() const; diff --git a/gtsam/geometry/Rot3M.h b/gtsam/geometry/Rot3M.h index 5f71b0668..e41e01ef3 100644 --- a/gtsam/geometry/Rot3M.h +++ b/gtsam/geometry/Rot3M.h @@ -152,11 +152,6 @@ namespace gtsam { Rot3M compose(const Rot3M& R2, boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /// MATLAB version returns shared pointer - boost::shared_ptr compose_(const Rot3M& p2) { - return boost::shared_ptr(new Rot3M(compose(p2))); - } - /// rotate point from rotated coordinate frame to world = R*p inline Point3 operator*(const Point3& p) const { return rotate(p);} @@ -182,11 +177,6 @@ namespace gtsam { /// Updates a with tangent space delta Rot3M retract(const Vector& v) const { return compose(Expmap(v)); } - /// MATLAB version returns shared pointer - boost::shared_ptr retract_(const Vector& v) { - return boost::shared_ptr(new Rot3M(retract(v))); - } - /// Returns inverse retraction Vector localCoordinates(const Rot3M& t2) const { return Logmap(between(t2)); } @@ -202,9 +192,6 @@ namespace gtsam { if(zero(v)) return Rot3M(); else return rodriguez(v); } - static inline boost::shared_ptr Expmap_(const Vector& v) { - return boost::shared_ptr(new Rot3M(Expmap(v))); - } /** * Log map at identity - return the canonical coordinates of this rotation @@ -260,11 +247,6 @@ namespace gtsam { boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /// MATLAB version returns shared pointer - boost::shared_ptr between_(const Rot3M& p2) { - return boost::shared_ptr(new Rot3M(between(p2))); - } - /** compose two rotations */ Rot3M operator*(const Rot3M& R2) const { return Rot3M(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_));