Classes | Private Member Functions | Private Attributes | List of all members
gtsam::Rot2 Class Reference

#include <Rot2.h>

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

Classes

struct  ChartAtOrigin
 

Public Member Functions

Testable
void print (const std::string &s="theta") const
 
bool equals (const Rot2 &R, double tol=1e-9) const
 
Group Action on Point2
Point2 rotate (const Point2 &p, OptionalJacobian< 2, 1 > H1=boost::none, OptionalJacobian< 2, 2 > H2=boost::none) const
 
Point2 operator* (const Point2 &p) const
 
Point2 unrotate (const Point2 &p, OptionalJacobian< 2, 1 > H1=boost::none, OptionalJacobian< 2, 2 > H2=boost::none) const
 
- Public Member Functions inherited from gtsam::LieGroup< Rot2, 1 >
Rot2 between (const Rot2 &g) const
 
Rot2 between (const Rot2 &g, ChartJacobian H1, ChartJacobian H2=boost::none) const
 
Rot2 compose (const Rot2 &g) const
 
Rot2 compose (const Rot2 &g, ChartJacobian H1, ChartJacobian H2=boost::none) const
 
const Rot2derived () const
 
Rot2 expmap (const TangentVector &v) const
 
Rot2 expmap (const TangentVector &v, ChartJacobian H1, ChartJacobian H2=boost::none) const
 expmap with optional derivatives More...
 
Rot2 inverse (ChartJacobian H) const
 
TangentVector localCoordinates (const Rot2 &g) const
 localCoordinates as required by manifold concept: finds tangent vector between *this and g More...
 
TangentVector localCoordinates (const Rot2 &g, ChartJacobian H1, ChartJacobian H2=boost::none) const
 localCoordinates with optional derivatives More...
 
TangentVector logmap (const Rot2 &g) const
 
TangentVector logmap (const Rot2 &g, ChartJacobian H1, ChartJacobian H2=boost::none) const
 logmap with optional derivatives More...
 
Rot2 retract (const TangentVector &v) const
 retract as required by manifold concept: applies v at *this More...
 
Rot2 retract (const TangentVector &v, ChartJacobian H1, ChartJacobian H2=boost::none) const
 retract with optional derivatives More...
 

Private Member Functions

Rot2normalize ()
 
 Rot2 (double c, double s)
 

Private Attributes

double c_
 
double s_
 

Constructors and named constructors

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

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=boost::none)
 Exponential map at identity - create a rotation from canonical coordinates. More...
 
static Vector1 Logmap (const Rot2 &r, ChartJacobian H=boost::none)
 Log map at identity - return the canonical coordinates of this rotation. More...
 
static Matrix ExpmapDerivative (const Vector &)
 Left-trivialized derivative of the exponential map. More...
 
static Matrix LogmapDerivative (const Vector &)
 Left-trivialized derivative inverse of the exponential map. More...
 

Standard Interface

class boost::serialization::access
 
template<class ARCHIVE >
void serialize (ARCHIVE &ar, const unsigned int)
 
Point2 unit () const
 Creates a unit vector as a Point2. More...
 
double theta () const
 
double degrees () const
 
double c () const
 
double s () const
 
Matrix2 matrix () const
 
Matrix2 transpose () const
 

Additional Inherited Members

- Public Types inherited from gtsam::LieGroup< Rot2, 1 >
enum  
 
typedef OptionalJacobian< N, NChartJacobian
 
typedef Eigen::Matrix< double, N, NJacobian
 
typedef Eigen::Matrix< double, N, 1 > TangentVector
 
- Static Public Member Functions inherited from gtsam::LieGroup< Rot2, 1 >
static TangentVector LocalCoordinates (const Rot2 &g)
 LocalCoordinates at origin: possible in Lie group because it has an identity. More...
 
static TangentVector LocalCoordinates (const Rot2 &g, ChartJacobian H)
 LocalCoordinates at origin with optional derivative. More...
 
static Rot2 Retract (const TangentVector &v)
 Retract at origin: possible in Lie group because it has an identity. More...
 
static Rot2 Retract (const TangentVector &v, ChartJacobian H)
 Retract at origin with optional derivative. More...
 

Detailed Description

Definition at line 35 of file Rot2.h.

Constructor & Destructor Documentation

gtsam::Rot2::Rot2 ( double  c,
double  s 
)
inlineprivate

private constructor from cos/sin

Definition at line 44 of file Rot2.h.

gtsam::Rot2::Rot2 ( )
inline

default constructor, zero rotation

Definition at line 52 of file Rot2.h.

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

Constructor from angle in radians == exponential map at identity.

Definition at line 55 of file Rot2.h.

Member Function Documentation

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

Calculate Adjoint map

Definition at line 128 of file Rot2.h.

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

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

Definition at line 37 of file Rot2.cpp.

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

return cos

Definition at line 194 of file Rot2.h.

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

return angle (DEGREES)

Definition at line 188 of file Rot2.h.

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

equals with an tolerance

Definition at line 55 of file Rot2.cpp.

Rot2 gtsam::Rot2::Expmap ( const Vector1 v,
ChartJacobian  H = boost::none 
)
static

Exponential map at identity - create a rotation from canonical coordinates.

Definition at line 71 of file Rot2.cpp.

static Matrix gtsam::Rot2::ExpmapDerivative ( const Vector )
inlinestatic

Left-trivialized derivative of the exponential map.

Definition at line 131 of file Rot2.h.

static Rot2 gtsam::Rot2::fromAngle ( double  theta)
inlinestatic

Named constructor from angle in radians.

Definition at line 58 of file Rot2.h.

Rot2 gtsam::Rot2::fromCosSin ( double  c,
double  s 
)
static

Named constructor from cos(theta),sin(theta) pair, will not normalize!

Definition at line 27 of file Rot2.cpp.

static Rot2 gtsam::Rot2::fromDegrees ( double  theta)
inlinestatic

Named constructor from angle in degrees.

Definition at line 63 of file Rot2.h.

static Rot2 gtsam::Rot2::identity ( )
inlinestatic

identity

Definition at line 107 of file Rot2.h.

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

The inverse rotation - negative angle

Definition at line 110 of file Rot2.h.

Vector1 gtsam::Rot2::Logmap ( const Rot2 r,
ChartJacobian  H = boost::none 
)
static

Log map at identity - return the canonical coordinates of this rotation.

Definition at line 81 of file Rot2.cpp.

static Matrix gtsam::Rot2::LogmapDerivative ( const Vector )
inlinestatic

Left-trivialized derivative inverse of the exponential map.

Definition at line 136 of file Rot2.h.

Matrix2 gtsam::Rot2::matrix ( ) const

return 2*2 rotation matrix

Definition at line 89 of file Rot2.cpp.

Rot2 & gtsam::Rot2::normalize ( void  )
private

normalize to make sure cos and sin form unit vector

Definition at line 60 of file Rot2.cpp.

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

Compose - make a new rotation by adding angles

Definition at line 113 of file Rot2.h.

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

syntactic sugar for rotate

Definition at line 163 of file Rot2.h.

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

print

Definition at line 50 of file Rot2.cpp.

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

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

Definition at line 43 of file Rot2.cpp.

Rot2 gtsam::Rot2::relativeBearing ( const Point2 d,
OptionalJacobian< 1, 2 >  H = boost::none 
)
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) $

Definition at line 123 of file Rot2.cpp.

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

rotate point from rotated coordinate frame to world $ p^w = R_c^w p^c $

Definition at line 104 of file Rot2.cpp.

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

return sin

Definition at line 199 of file Rot2.h.

template<class ARCHIVE >
void gtsam::Rot2::serialize ( ARCHIVE &  ar,
const unsigned  int 
)
inlineprivate

Definition at line 213 of file Rot2.h.

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

return angle (RADIANS)

Definition at line 183 of file Rot2.h.

Matrix2 gtsam::Rot2::transpose ( ) const

return 2*2 transpose (inverse) rotation matrix

Definition at line 96 of file Rot2.cpp.

Point2 gtsam::Rot2::unit ( ) const
inline

Creates a unit vector as a Point2.

Definition at line 178 of file Rot2.h.

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

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

Definition at line 114 of file Rot2.cpp.

Friends And Related Function Documentation

friend class boost::serialization::access
friend

Serialization function

Definition at line 211 of file Rot2.h.

Member Data Documentation

double gtsam::Rot2::c_
private

we store cos(theta) and sin(theta)

Definition at line 38 of file Rot2.h.

double gtsam::Rot2::s_
private

Definition at line 38 of file Rot2.h.


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


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:58:27