GTSAM  4.0.2
C++ library for smoothing and mapping (SAM)
Classes | Public Types | Public Member Functions | Static Public Member Functions | List of all members
gtsam::Rot2 Class Reference

#include <Rot2.h>

Inheritance diagram for gtsam::Rot2:
Inheritance graph
[legend]
Collaboration diagram for gtsam::Rot2:
Collaboration graph
[legend]

Classes

struct  ChartAtOrigin
 

Public Types

enum  
 
typedef OptionalJacobian< N, N > ChartJacobian
 
typedef Eigen::Matrix< double, N, N > Jacobian
 
typedef Eigen::Matrix< double, N, 1 > TangentVector
 

Public Member Functions

const Rot2derived () const
 
Rot2 compose (const Rot2 &g) const
 
Rot2 compose (const Rot2 &g, ChartJacobian H1, ChartJacobian H2={}) const
 
GTSAM_EXPORT SOn compose (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const
 
Rot2 between (const Rot2 &g) const
 
Rot2 between (const Rot2 &g, ChartJacobian H1, ChartJacobian H2={}) const
 
GTSAM_EXPORT SOn between (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const
 
Rot2 inverse (ChartJacobian H) const
 
Rot2 expmap (const TangentVector &v) const
 
Rot2 expmap (const TangentVector &v, ChartJacobian H1, ChartJacobian H2={}) const
 expmap with optional derivatives
 
TangentVector logmap (const Rot2 &g) const
 
TangentVector logmap (const Rot2 &g, ChartJacobian H1, ChartJacobian H2={}) const
 logmap with optional derivatives
 
Rot2 retract (const TangentVector &v) const
 retract as required by manifold concept: applies v at *this
 
Rot2 retract (const TangentVector &v, ChartJacobian H1, ChartJacobian H2={}) const
 retract with optional derivatives
 
TangentVector localCoordinates (const Rot2 &g) const
 localCoordinates as required by manifold concept: finds tangent vector between *this and g
 
TangentVector localCoordinates (const Rot2 &g, ChartJacobian H1, ChartJacobian H2={}) const
 localCoordinates with optional derivatives
 

Static Public Member Functions

static Rot2 Retract (const TangentVector &v)
 Retract at origin: possible in Lie group because it has an identity.
 
static Rot2 Retract (const TangentVector &v, ChartJacobian H)
 Retract at origin with optional derivative.
 
static TangentVector LocalCoordinates (const Rot2 &g)
 LocalCoordinates at origin: possible in Lie group because it has an identity.
 
static TangentVector LocalCoordinates (const Rot2 &g, ChartJacobian H)
 LocalCoordinates at origin with optional derivative.
 

Constructors and named constructors

 Rot2 ()
 
 Rot2 (const Rot2 &r)
 
 Rot2 (double theta)
 Constructor from angle in radians == exponential map at identity.
 
static Rot2 fromAngle (double theta)
 Named constructor from angle in radians.
 
static Rot2 fromDegrees (double theta)
 Named constructor from angle in degrees.
 
static Rot2 fromCosSin (double c, double s)
 Named constructor from cos(theta),sin(theta) pair, will not normalize!
 
static Rot2 relativeBearing (const Point2 &d, OptionalJacobian< 1, 2 > H={})
 
static Rot2 atan2 (double y, double x)
 
static Rot2 Random (std::mt19937 &rng)
 

Testable

void print (const std::string &s="theta") const
 
bool equals (const Rot2 &R, double tol=1e-9) const
 

Group

Rot2 inverse () const
 
Rot2 operator* (const Rot2 &R) const
 
static Rot2 Identity ()
 

Lie Group

Matrix1 AdjointMap () const
 
static Rot2 Expmap (const Vector1 &v, ChartJacobian H={})
 Exponential map at identity - create a rotation from canonical coordinates.
 
static Vector1 Logmap (const Rot2 &r, ChartJacobian H={})
 Log map at identity - return the canonical coordinates of this rotation.
 
static Matrix ExpmapDerivative (const Vector &)
 Left-trivialized derivative of the exponential map.
 
static Matrix LogmapDerivative (const Vector &)
 Left-trivialized derivative inverse of the exponential map.
 

Group Action on Point2

Point2 rotate (const Point2 &p, OptionalJacobian< 2, 1 > H1={}, OptionalJacobian< 2, 2 > H2={}) const
 
Point2 operator* (const Point2 &p) const
 
Point2 unrotate (const Point2 &p, OptionalJacobian< 2, 1 > H1={}, OptionalJacobian< 2, 2 > H2={}) const
 

Standard Interface

Point2 unit () const
 Creates a unit vector as a Point2.
 
double theta () const
 
double degrees () const
 
double c () const
 
double s () const
 
Matrix2 matrix () const
 
Matrix2 transpose () const
 
static Rot2 ClosestTo (const Matrix2 &M)
 

Detailed Description

Rotation matrix NOTE: the angle theta is in radians unless explicitly stated

Constructor & Destructor Documentation

◆ Rot2() [1/2]

gtsam::Rot2::Rot2 ( )
inline

default constructor, zero rotation

◆ Rot2() [2/2]

gtsam::Rot2::Rot2 ( const Rot2 r)
inline

copy constructor

Member Function Documentation

◆ AdjointMap()

Matrix1 gtsam::Rot2::AdjointMap ( ) const
inline

Calculate Adjoint map

◆ atan2()

static Rot2 gtsam::Rot2::atan2 ( double  y,
double  x 
)
static

Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes

◆ c()

double gtsam::Rot2::c ( ) const
inline

return cos

◆ ClosestTo()

static Rot2 gtsam::Rot2::ClosestTo ( const Matrix2 &  M)
static

Find closest valid rotation matrix, given a 2x2 matrix

◆ degrees()

double gtsam::Rot2::degrees ( ) const
inline

return angle (DEGREES)

◆ equals()

bool gtsam::Rot2::equals ( const Rot2 R,
double  tol = 1e-9 
) const

equals with an tolerance

◆ expmap()

Rot2 gtsam::LieGroup< Rot2 , N >::expmap ( const TangentVector &  v) const
inlineinherited

expmap as required by manifold concept Applies exponential map to v and composes with *this

◆ Identity()

static Rot2 gtsam::Rot2::Identity ( )
inlinestatic

Identity

◆ inverse()

Rot2 gtsam::Rot2::inverse ( ) const
inline

The inverse rotation - negative angle

◆ logmap()

TangentVector gtsam::LieGroup< Rot2 , N >::logmap ( const Rot2 g) const
inlineinherited

logmap as required by manifold concept Applies logarithmic map to group element that takes *this to g

◆ matrix()

Matrix2 gtsam::Rot2::matrix ( ) const

return 2*2 rotation matrix

◆ operator*() [1/2]

Rot2 gtsam::Rot2::operator* ( const Rot2 R) const
inline

Compose - make a new rotation by adding angles

◆ operator*() [2/2]

Point2 gtsam::Rot2::operator* ( const Point2 p) const
inline

syntactic sugar for rotate

◆ print()

void gtsam::Rot2::print ( const std::string &  s = "theta") const

print

◆ Random()

static Rot2 gtsam::Rot2::Random ( std::mt19937 &  rng)
static

Random, generates random angle \(\in\) [-pi,pi] Example: std::mt19937 engine(42); Unit3 unit = Unit3::Random(engine);

◆ relativeBearing()

static Rot2 gtsam::Rot2::relativeBearing ( const Point2 d,
OptionalJacobian< 1, 2 >  H = {} 
)
static

Named constructor with derivative Calculate relative bearing to a landmark in local coordinate frame

Parameters
d2D location of landmark
Hoptional reference for Jacobian
Returns
2D rotation \( \in SO(2) \)

◆ rotate()

Point2 gtsam::Rot2::rotate ( const Point2 p,
OptionalJacobian< 2, 1 >  H1 = {},
OptionalJacobian< 2, 2 >  H2 = {} 
) const

rotate point from rotated coordinate frame to world \( p^w = R_c^w p^c \)

◆ s()

double gtsam::Rot2::s ( ) const
inline

return sin

◆ theta()

double gtsam::Rot2::theta ( ) const
inline

return angle (RADIANS)

◆ transpose()

Matrix2 gtsam::Rot2::transpose ( ) const

return 2*2 transpose (inverse) rotation matrix

◆ unrotate()

Point2 gtsam::Rot2::unrotate ( const Point2 p,
OptionalJacobian< 2, 1 >  H1 = {},
OptionalJacobian< 2, 2 >  H2 = {} 
) const

rotate point from world to rotated frame \( p^c = (R_c^w)^T p^w \)


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