gtsam  3.2.0
gtsam
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
gtsam::Rot3 Class Reference
+ Inheritance diagram for gtsam::Rot3:

Constructors and named constructors

 Rot3 ()
 default constructor, unit rotation
 
 Rot3 (const Point3 &col1, const Point3 &col2, const Point3 &col3)
 Constructor from columns More...
 
 Rot3 (double R11, double R12, double R13, double R21, double R22, double R23, double R31, double R32, double R33)
 constructor from a rotation matrix, as doubles in row-major order !!!
 
 Rot3 (const Matrix3 &R)
 constructor from a rotation matrix
 
 Rot3 (const Matrix &R)
 constructor from a rotation matrix
 
 Rot3 (const Quaternion &q)
 Constructor from a quaternion. More...
 
virtual ~Rot3 ()
 Virtual destructor.
 
static Rot3 Random (boost::mt19937 &rng)
 Random, generates a random axis, then random angle [-p,pi].
 
static Rot3 Rx (double t)
 Rotation around X axis as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis.
 
static Rot3 Ry (double t)
 Rotation around Y axis as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis.
 
static Rot3 Rz (double t)
 Rotation around Z axis as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis.
 
static Rot3 RzRyRx (double x, double y, double z)
 Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis.
 
static Rot3 RzRyRx (const Vector &xyz)
 Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis.
 
static Rot3 yaw (double t)
 Positive yaw is to right (as in aircraft heading). See ypr.
 
static Rot3 pitch (double t)
 Positive pitch is up (increasing aircraft altitude).See ypr.
 
static Rot3 roll (double t)
 
static Rot3 ypr (double y, double p, double r)
 Returns rotation nRb from body to nav frame. More...
 
static Rot3 quaternion (double w, double x, double y, double z)
 Create from Quaternion coefficients.
 
static Rot3 rodriguez (const Vector &w, double theta)
 Rodriguez' formula to compute an incremental rotation matrix. More...
 
static Rot3 rodriguez (const Point3 &w, double theta)
 Rodriguez' formula to compute an incremental rotation matrix. More...
 
static Rot3 rodriguez (const Unit3 &w, double theta)
 Rodriguez' formula to compute an incremental rotation matrix. More...
 
static Rot3 rodriguez (const Vector &v)
 Rodriguez' formula to compute an incremental rotation matrix. More...
 
static Rot3 rodriguez (double wx, double wy, double wz)
 Rodriguez' formula to compute an incremental rotation matrix. More...
 

Group

Rot3 inverse (boost::optional< Matrix & > H1=boost::none) const
 derivative of inverse rotation R^T s.t. inverse(R)*R = identity
 
Rot3 compose (const Rot3 &R2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const
 Compose two rotations i.e., R= (*this) * R2.
 
Rot3 operator* (const Rot3 &R2) const
 compose two rotations
 
Rot3 conjugate (const Rot3 &cRb) const
 Conjugation: given a rotation acting in frame B, compute rotation c1Rc2 acting in a frame C. More...
 
Rot3 between (const Rot3 &R2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const
 Return relative rotation D s.t. More...
 
static Rot3 identity ()
 identity rotation for group operation
 

Manifold

enum  CoordinatesMode { EXPMAP, CAYLEY, SLOW_CAYLEY }
 The method retract() is used to map from the tangent space back to the manifold. More...
 
size_t dim () const
 return dimensionality of tangent space, DOF = 3
 
Rot3 retractCayley (const Vector &omega) const
 Retraction from R^3 to Rot3 manifold using the Cayley transform.
 
Rot3 retract (const Vector &omega, Rot3::CoordinatesMode mode=ROT3_DEFAULT_COORDINATES_MODE) const
 Retraction from R^3 \( [R_x,R_y,R_z] \) to Rot3 manifold neighborhood around current rotation.
 
Vector3 localCoordinates (const Rot3 &t2, Rot3::CoordinatesMode mode=ROT3_DEFAULT_COORDINATES_MODE) const
 Returns local retract coordinates \( [R_x,R_y,R_z] \) in neighborhood around current rotation.
 
static size_t Dim ()
 dimension of the variable - used to autodetect sizes
 

Advanced Interface

class boost::serialization::access
 Serialization function.
 
GTSAM_EXPORT friend std::ostream & operator<< (std::ostream &os, const Rot3 &p)
 Output stream operator.
 
Quaternion toQuaternion () const
 Compute the quaternion representation of this rotation. More...
 
Vector quaternion () const
 Converts to a generic matrix to allow for use with matlab In format: w x y z.
 

Public Member Functions

Testable
void print (const std::string &s="R") const
 print
 
bool equals (const Rot3 &p, double tol=1e-9) const
 equals with an tolerance
 
Group Action on Point3
Point3 rotate (const Point3 &p, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const
 rotate point from rotated coordinate frame to world \( p^w = R_c^w p^c \)
 
Point3 operator* (const Point3 &p) const
 rotate point from rotated coordinate frame to world = R*p
 
Point3 unrotate (const Point3 &p, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const
 rotate point from world to rotated frame \( p^c = (R_c^w)^T p^w \)
 
Group Action on Unit3
Unit3 rotate (const Unit3 &p, boost::optional< Matrix & > HR=boost::none, boost::optional< Matrix & > Hp=boost::none) const
 rotate 3D direction from rotated coordinate frame to world frame
 
Unit3 unrotate (const Unit3 &p, boost::optional< Matrix & > HR=boost::none, boost::optional< Matrix & > Hp=boost::none) const
 unrotate 3D direction from world frame to rotated coordinate frame
 
Unit3 operator* (const Unit3 &p) const
 rotate 3D direction from rotated coordinate frame to world frame
 
Standard Interface
Matrix3 matrix () const
 return 3*3 rotation matrix
 
Matrix3 transpose () const
 return 3*3 transpose (inverse) rotation matrix
 
Point3 column (int index) const
 
Point3 r1 () const
 first column
 
Point3 r2 () const
 second column
 
Point3 r3 () const
 third column
 
Vector3 xyz () const
 Use RQ to calculate xyz angle representation. More...
 
Vector3 ypr () const
 Use RQ to calculate yaw-pitch-roll angle representation. More...
 
Vector3 rpy () const
 Use RQ to calculate roll-pitch-yaw angle representation. More...
 
double roll () const
 Accessor to get to component 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.
 
double pitch () const
 Accessor to get to component 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.
 
double yaw () const
 Accessor to get to component 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.
 
- Public Member Functions inherited from gtsam::DerivedValue< Rot3 >
virtual Valueclone_ () const
 Create a duplicate object returned as a pointer to the generic Value interface. More...
 
virtual void deallocate_ () const
 Destroy and deallocate this object, only if it was originally allocated using clone_().
 
virtual boost::shared_ptr< Valueclone () const
 Clone this value (normal clone on the heap, delete with 'delete' operator)
 
virtual bool equals_ (const Value &p, double tol=1e-9) const
 equals implementing generic Value interface
 
virtual Valueretract_ (const Vector &delta) const
 Generic Value interface version of retract.
 
virtual Vector localCoordinates_ (const Value &value2) const
 Generic Value interface version of localCoordinates.
 
virtual Valueoperator= (const Value &rhs)
 Assignment operator.
 
 operator const Rot3 & () const
 Conversion to the derived class.
 
 operator Rot3 & ()
 Conversion to the derived class.
 
- Public Member Functions inherited from gtsam::Value
virtual ~Value ()
 Virutal destructor.
 

Static Public Member Functions

Lie Group
static Rot3 Expmap (const Vector &v)
 Exponential map at identity - create a rotation from canonical coordinates \( [R_x,R_y,R_z] \) using Rodriguez' formula.
 
static Vector3 Logmap (const Rot3 &R)
 Log map at identity - return the canonical coordinates \( [R_x,R_y,R_z] \) of this rotation.
 
static Matrix3 dexpL (const Vector3 &v)
 Left-trivialized derivative of the exponential map. More...
 
static Matrix3 dexpInvL (const Vector3 &v)
 Left-trivialized derivative inverse of the exponential map. More...
 
static Matrix3 rightJacobianExpMapSO3 (const Vector3 &x)
 Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in G.S. More...
 
static Matrix3 rightJacobianExpMapSO3inverse (const Vector3 &x)
 Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in G.S. More...
 

Static Public Attributes

static const size_t dimension = 3
 

Additional Inherited Members

- Protected Member Functions inherited from gtsam::DerivedValue< Rot3 >
DerivedValue< Rot3 > & operator= (const DerivedValue< Rot3 > &rhs)
 Assignment operator, protected because only the Value or DERIVED assignment operators should be used. More...
 

Member Enumeration Documentation

The method retract() is used to map from the tangent space back to the manifold.

Its inverse, is localCoordinates(). For Lie groups, an obvious retraction is the exponential map, but this can be expensive to compute. The following Enum is used to indicate which method should be used. The default is determined by ROT3_DEFAULT_COORDINATES_MODE, which may be set at compile time, and itself defaults to Rot3::CAYLEY, or if GTSAM_USE_QUATERNIONS is defined, to Rot3::EXPMAP.

Enumerator
EXPMAP 

Use the Lie group exponential map to retract.

CAYLEY 

Retract and localCoordinates using the Cayley transform.

SLOW_CAYLEY 

Slow matrix implementation of Cayley transform (for tests only).

Constructor & Destructor Documentation

gtsam::Rot3::Rot3 ( const Point3 col1,
const Point3 col2,
const Point3 col3 
)

Constructor from columns

Parameters
r1X-axis of rotated frame
r2Y-axis of rotated frame
r3Z-axis of rotated frame
gtsam::Rot3::Rot3 ( const Quaternion q)

Constructor from a quaternion.

This can also be called using a plain Vector, due to implicit conversion from Vector to Quaternion

Parameters
qThe quaternion
  • ************************************************************************* */

Member Function Documentation

Rot3 gtsam::Rot3::between ( const Rot3 R2,
boost::optional< Matrix & >  H1 = boost::none,
boost::optional< Matrix & >  H2 = boost::none 
) const

Return relative rotation D s.t.

R2=D*R1, i.e. D=R2*R1'

Point3 gtsam::Rot3::column ( int  index) const
Deprecated:
, this is base 1, and was just confusing
Rot3 gtsam::Rot3::conjugate ( const Rot3 cRb) const
inline

Conjugation: given a rotation acting in frame B, compute rotation c1Rc2 acting in a frame C.

Parameters
cRbrotation from B frame to C frame
Returns
c1Rc2 = cRb * b1Rb2 * cRb'
Matrix3 gtsam::Rot3::dexpInvL ( const Vector3 &  v)
static

Left-trivialized derivative inverse of the exponential map.

Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)

Matrix3 gtsam::Rot3::dexpL ( const Vector3 &  v)
static

Left-trivialized derivative of the exponential map.

Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)

Matrix3 gtsam::Rot3::rightJacobianExpMapSO3 ( const Vector3 &  x)
static

Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in G.S.

Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.

Matrix3 gtsam::Rot3::rightJacobianExpMapSO3inverse ( const Vector3 &  x)
static

Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in G.S.

Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.

Rot3 gtsam::Rot3::rodriguez ( const Vector &  w,
double  theta 
)
static

Rodriguez' formula to compute an incremental rotation matrix.

Parameters
wis the rotation axis, unit length
thetarotation angle
Returns
incremental rotation matrix
Rot3 gtsam::Rot3::rodriguez ( const Point3 w,
double  theta 
)
static

Rodriguez' formula to compute an incremental rotation matrix.

Parameters
wis the rotation axis, unit length
thetarotation angle
Returns
incremental rotation matrix
Rot3 gtsam::Rot3::rodriguez ( const Unit3 w,
double  theta 
)
static

Rodriguez' formula to compute an incremental rotation matrix.

Parameters
wis the rotation axis
thetarotation angle
Returns
incremental rotation matrix
Rot3 gtsam::Rot3::rodriguez ( const Vector &  v)
static

Rodriguez' formula to compute an incremental rotation matrix.

Parameters
va vector of incremental roll,pitch,yaw
Returns
incremental rotation matrix
static Rot3 gtsam::Rot3::rodriguez ( double  wx,
double  wy,
double  wz 
)
inlinestatic

Rodriguez' formula to compute an incremental rotation matrix.

Parameters
wxIncremental roll (about X)
wyIncremental pitch (about Y)
wzIncremental yaw (about Z)
Returns
incremental rotation matrix
Vector3 gtsam::Rot3::rpy ( ) const

Use RQ to calculate roll-pitch-yaw angle representation.

Returns
a vector containing ypr s.t. R = Rot3::ypr(y,p,r)
Quaternion gtsam::Rot3::toQuaternion ( ) const

Compute the quaternion representation of this rotation.

Returns
The quaternion
Vector3 gtsam::Rot3::xyz ( ) const

Use RQ to calculate xyz angle representation.

Returns
a vector containing x,y,z s.t. R = Rot3::RzRyRx(x,y,z)
static Rot3 gtsam::Rot3::ypr ( double  y,
double  p,
double  r 
)
inlinestatic

Returns rotation nRb from body to nav frame.

Positive yaw is to right (as in aircraft heading). Positive pitch is up (increasing aircraft altitude). Positive roll is to right (increasing yaw in aircraft). Tait-Bryan system from Spatial Reference Model (SRM) (x,y,z) = (roll,pitch,yaw) as described in http://www.sedris.org/wg8home/Documents/WG80462.pdf. Assumes vehicle coordinate frame X forward, Y right, Z down.

Vector3 gtsam::Rot3::ypr ( ) const

Use RQ to calculate yaw-pitch-roll angle representation.

Returns
a vector containing ypr s.t. R = Rot3::ypr(y,p,r)

The documentation for this class was generated from the following files: