diff --git a/gtsam.h b/gtsam.h index 432c95698..356914127 100644 --- a/gtsam.h +++ b/gtsam.h @@ -107,6 +107,9 @@ class Rot3 { Matrix transpose() const; Vector xyz() const; Vector ypr() const; + double roll() const; + double pitch() const; + double yaw() const; // Vector toQuaternion() const; // FIXME: Can't cast to Vector properly void print(string s) const; bool equals(const Rot3& rot, double tol) const; @@ -134,6 +137,8 @@ class Pose2 { Pose2 retract(Vector v); Pose2 compose(const Pose2& p2); Pose2 between(const Pose2& p2); + Rot2 bearing(const Point2& point); + double range(const Point2& point); }; class Pose3 { diff --git a/gtsam/geometry/Rot3M.h b/gtsam/geometry/Rot3M.h index e41e01ef3..02ca733d1 100644 --- a/gtsam/geometry/Rot3M.h +++ b/gtsam/geometry/Rot3M.h @@ -226,10 +226,20 @@ namespace gtsam { /** * Use RQ to calculate roll-pitch-yaw angle representation - * @return a vector containing ypr s.t. R = Rot3M::ypr(y,p,r) + * @return a vector containing rpy s.t. R = Rot3M::ypr(y,p,r) */ Vector rpy() const; + /** + * Accessors to get to components of angle representations + * NOTE: these are not efficient to get to multiple separate parts, + * you should instead use xyz() or ypr() + * TODO: make this more efficient + */ + inline double roll() const { return ypr()(2); } + inline double pitch() const { return ypr()(1); } + inline double yaw() const { return ypr()(0); } + /** Compute the quaternion representation of this rotation. * @return The quaternion */