12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added syntactic sugar for access to yaw, pitch, roll from Rot3M with wrap access

release/4.3a0
Alex Cunningham 2011-12-07 16:41:50 +00:00
parent 886f9459b4
commit f9ef0e479c
2 changed files with 16 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam.h
View File

@ -107,6 +107,9 @@ class Rot3 {
Matrix transpose() const; Matrix transpose() const;
Vector xyz() const; Vector xyz() const;
Vector ypr() const; Vector ypr() const;
double roll() const;
double pitch() const;
double yaw() const;
// Vector toQuaternion() const; // FIXME: Can't cast to Vector properly // Vector toQuaternion() const; // FIXME: Can't cast to Vector properly
void print(string s) const; void print(string s) const;
bool equals(const Rot3& rot, double tol) const; bool equals(const Rot3& rot, double tol) const;
@ -134,6 +137,8 @@ class Pose2 {
Pose2 retract(Vector v); Pose2 retract(Vector v);
Pose2 compose(const Pose2& p2); Pose2 compose(const Pose2& p2);
Pose2 between(const Pose2& p2); Pose2 between(const Pose2& p2);
Rot2 bearing(const Point2& point);
double range(const Point2& point);
}; };
class Pose3 { class Pose3 {

12
gtsam/geometry/Rot3M.h
View File

@ -226,10 +226,20 @@ namespace gtsam {
/** /**
* Use RQ to calculate roll-pitch-yaw angle representation * 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; 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. /** Compute the quaternion representation of this rotation.
* @return The quaternion * @return The quaternion
*/ */