Classes | Public Types | Private Attributes | List of all members
gtsam::Pose2 Class Reference

#include <Pose2.h>

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

Classes

struct  ChartAtOrigin
 

Public Types

typedef Rot2 Rotation
 
typedef Point2 Translation
 
- Public Types inherited from gtsam::LieGroup< Pose2, 3 >
enum  
 
typedef OptionalJacobian< N, NChartJacobian
 
typedef Eigen::Matrix< double, N, NJacobian
 
typedef Eigen::Matrix< double, N, 1 > TangentVector
 

Private Attributes

Rot2 r_
 
Point2 t_
 

Standard Constructors

 Pose2 ()
 
 Pose2 (const Pose2 &pose)
 
 Pose2 (double x, double y, double theta)
 
 Pose2 (double theta, const Point2 &t)
 
 Pose2 (const Rot2 &r, const Point2 &t)
 
 Pose2 (const Matrix &T)
 

Advanced Constructors

 Pose2 (const Vector &v)
 
static std::optional< Pose2Align (const Point2Pairs &abPointPairs)
 
static std::optional< Pose2Align (const Matrix &a, const Matrix &b)
 

Testable

GTSAM_EXPORT void print (const std::string &s="") const
 
GTSAM_EXPORT bool equals (const Pose2 &pose, double tol=1e-9) const
 

Group

GTSAM_EXPORT Pose2 inverse () const
 inverse More...
 
Pose2 operator* (const Pose2 &p2) const
 compose syntactic sugar More...
 
static Pose2 Identity ()
 identity for group operation More...
 

Lie Group

GTSAM_EXPORT Matrix3 AdjointMap () const
 
Vector3 Adjoint (const Vector3 &xi) const
 Apply AdjointMap to twist xi. More...
 
static GTSAM_EXPORT Pose2 Expmap (const Vector3 &xi, ChartJacobian H={})
 Exponential map at identity - create a rotation from canonical coordinates $ [T_x,T_y,\theta] $. More...
 
static GTSAM_EXPORT Vector3 Logmap (const Pose2 &p, ChartJacobian H={})
 Log map at identity - return the canonical coordinates $ [T_x,T_y,\theta] $ of this rotation. More...
 
static GTSAM_EXPORT Matrix3 adjointMap (const Vector3 &v)
 
static Vector3 adjoint (const Vector3 &xi, const Vector3 &y)
 
static Vector3 adjointTranspose (const Vector3 &xi, const Vector3 &y)
 
static Matrix3 adjointMap_ (const Vector3 &xi)
 
static Vector3 adjoint_ (const Vector3 &xi, const Vector3 &y)
 
static Matrix3 wedge (double vx, double vy, double w)
 
static GTSAM_EXPORT Matrix3 ExpmapDerivative (const Vector3 &v)
 Derivative of Expmap. More...
 
static GTSAM_EXPORT Matrix3 LogmapDerivative (const Pose2 &v)
 Derivative of Logmap. More...
 

Group Action on Point2

GTSAM_EXPORT Point2 transformTo (const Point2 &point, OptionalJacobian< 2, 3 > Dpose={}, OptionalJacobian< 2, 2 > Dpoint={}) const
 
Matrix transformTo (const Matrix &points) const
 transform many points in world coordinates and transform to Pose. More...
 
GTSAM_EXPORT Point2 transformFrom (const Point2 &point, OptionalJacobian< 2, 3 > Dpose={}, OptionalJacobian< 2, 2 > Dpoint={}) const
 
Matrix transformFrom (const Matrix &points) const
 transform many points in Pose coordinates and transform to world. More...
 
Point2 operator* (const Point2 &point) const
 

Standard Interface

double x () const
 get x More...
 
double y () const
 get y More...
 
double theta () const
 get theta More...
 
const Point2t () const
 translation More...
 
const Rot2r () const
 rotation More...
 
const Point2translation (OptionalJacobian< 2, 3 > Hself={}) const
 translation More...
 
const Rot2rotation (OptionalJacobian< 1, 3 > Hself={}) const
 rotation More...
 
GTSAM_EXPORT Matrix3 matrix () const
 
GTSAM_EXPORT Rot2 bearing (const Point2 &point, OptionalJacobian< 1, 3 > H1={}, OptionalJacobian< 1, 2 > H2={}) const
 
GTSAM_EXPORT Rot2 bearing (const Pose2 &pose, OptionalJacobian< 1, 3 > H1={}, OptionalJacobian< 1, 3 > H2={}) const
 
GTSAM_EXPORT double range (const Point2 &point, OptionalJacobian< 1, 3 > H1={}, OptionalJacobian< 1, 2 > H2={}) const
 
GTSAM_EXPORT double range (const Pose2 &point, OptionalJacobian< 1, 3 > H1={}, OptionalJacobian< 1, 3 > H2={}) const
 

Advanced Interface

static std::pair< size_t, size_ttranslationInterval ()
 
static std::pair< size_t, size_trotationInterval ()
 
GTSAM_EXPORT friend std::ostream & operator<< (std::ostream &os, const Pose2 &p)
 Output stream operator. More...
 

Additional Inherited Members

- Public Member Functions inherited from gtsam::LieGroup< Pose2, 3 >
Pose2 between (const Pose2 &g) const
 
Pose2 between (const Pose2 &g, ChartJacobian H1, ChartJacobian H2={}) const
 
SOn between (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const
 
GTSAM_EXPORT SOn between (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const
 
Pose2 compose (const Pose2 &g) const
 
Pose2 compose (const Pose2 &g, ChartJacobian H1, ChartJacobian H2={}) const
 
SOn compose (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const
 
GTSAM_EXPORT SOn compose (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const
 
const Pose2derived () const
 
Pose2 expmap (const TangentVector &v) const
 
Pose2 expmap (const TangentVector &v, ChartJacobian H1, ChartJacobian H2={}) const
 expmap with optional derivatives More...
 
Pose2 inverse (ChartJacobian H) const
 
TangentVector localCoordinates (const Pose2 &g) const
 localCoordinates as required by manifold concept: finds tangent vector between *this and g More...
 
TangentVector localCoordinates (const Pose2 &g, ChartJacobian H1, ChartJacobian H2={}) const
 localCoordinates with optional derivatives More...
 
TangentVector logmap (const Pose2 &g) const
 
TangentVector logmap (const Pose2 &g, ChartJacobian H1, ChartJacobian H2={}) const
 logmap with optional derivatives More...
 
Pose2 retract (const TangentVector &v) const
 retract as required by manifold concept: applies v at *this More...
 
Pose2 retract (const TangentVector &v, ChartJacobian H1, ChartJacobian H2={}) const
 retract with optional derivatives More...
 
- Static Public Member Functions inherited from gtsam::LieGroup< Pose2, 3 >
static TangentVector LocalCoordinates (const Pose2 &g)
 LocalCoordinates at origin: possible in Lie group because it has an identity. More...
 
static TangentVector LocalCoordinates (const Pose2 &g, ChartJacobian H)
 LocalCoordinates at origin with optional derivative. More...
 
static Pose2 Retract (const TangentVector &v)
 Retract at origin: possible in Lie group because it has an identity. More...
 
static Pose2 Retract (const TangentVector &v, ChartJacobian H)
 Retract at origin with optional derivative. More...
 

Detailed Description

A 2D pose (Point2,Rot2)

Definition at line 39 of file Pose2.h.

Member Typedef Documentation

◆ Rotation

Pose Concept requirements

Definition at line 44 of file Pose2.h.

◆ Translation

Definition at line 45 of file Pose2.h.

Constructor & Destructor Documentation

◆ Pose2() [1/7]

gtsam::Pose2::Pose2 ( )
inline

default constructor = origin

Definition at line 58 of file Pose2.h.

◆ Pose2() [2/7]

gtsam::Pose2::Pose2 ( const Pose2 pose)
inline

copy constructor

Definition at line 63 of file Pose2.h.

◆ Pose2() [3/7]

gtsam::Pose2::Pose2 ( double  x,
double  y,
double  theta 
)
inline

construct from (x,y,theta)

Parameters
xx coordinate
yy coordinate
thetaangle with positive X-axis

Definition at line 71 of file Pose2.h.

◆ Pose2() [4/7]

gtsam::Pose2::Pose2 ( double  theta,
const Point2 t 
)
inline

construct from rotation and translation

Definition at line 76 of file Pose2.h.

◆ Pose2() [5/7]

gtsam::Pose2::Pose2 ( const Rot2 r,
const Point2 t 
)
inline

construct from r,t

Definition at line 81 of file Pose2.h.

◆ Pose2() [6/7]

gtsam::Pose2::Pose2 ( const Matrix T)
inline

Constructor from 3*3 matrix

Definition at line 84 of file Pose2.h.

◆ Pose2() [7/7]

gtsam::Pose2::Pose2 ( const Vector v)
inline

Construct from canonical coordinates $ [T_x,T_y,\theta] $ (Lie algebra)

Definition at line 94 of file Pose2.h.

Member Function Documentation

◆ Adjoint()

Vector3 gtsam::Pose2::Adjoint ( const Vector3 xi) const
inline

Apply AdjointMap to twist xi.

Definition at line 152 of file Pose2.h.

◆ adjoint()

static Vector3 gtsam::Pose2::adjoint ( const Vector3 xi,
const Vector3 y 
)
inlinestatic

Action of the adjointMap on a Lie-algebra vector y, with optional derivatives

Definition at line 164 of file Pose2.h.

◆ adjoint_()

static Vector3 gtsam::Pose2::adjoint_ ( const Vector3 xi,
const Vector3 y 
)
inlinestatic

Definition at line 177 of file Pose2.h.

◆ AdjointMap()

Matrix3 gtsam::Pose2::AdjointMap ( ) const

Calculate Adjoint map Ad_pose is 3*3 matrix that when applied to twist xi $ [T_x,T_y,\theta] $, returns Ad_pose(xi)

Definition at line 126 of file Pose2.cpp.

◆ adjointMap()

Matrix3 gtsam::Pose2::adjointMap ( const Vector3 v)
static

Compute the [ad(w,v)] operator for SE2 as in [Kobilarov09siggraph], pg 19

Definition at line 137 of file Pose2.cpp.

◆ adjointMap_()

static Matrix3 gtsam::Pose2::adjointMap_ ( const Vector3 xi)
inlinestatic

Definition at line 176 of file Pose2.h.

◆ adjointTranspose()

static Vector3 gtsam::Pose2::adjointTranspose ( const Vector3 xi,
const Vector3 y 
)
inlinestatic

The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.

Definition at line 171 of file Pose2.h.

◆ Align() [1/2]

std::optional< Pose2 > gtsam::Pose2::Align ( const Point2Pairs abPointPairs)
static

Create Pose2 by aligning two point pairs A pose aTb is estimated between pairs (a_point, b_point) such that a_point = aTb * b_point Note this allows for noise on the points but in that case the mapping will not be exact.

Definition at line 330 of file Pose2.cpp.

◆ Align() [2/2]

std::optional< Pose2 > gtsam::Pose2::Align ( const Matrix a,
const Matrix b 
)
static

Definition at line 362 of file Pose2.cpp.

◆ bearing() [1/2]

Rot2 gtsam::Pose2::bearing ( const Point2 point,
OptionalJacobian< 1, 3 >  H1 = {},
OptionalJacobian< 1, 2 >  H2 = {} 
) const

Calculate bearing to a landmark

Parameters
point2D location of landmark
Returns
2D rotation $ \in SO(2) $

Definition at line 245 of file Pose2.cpp.

◆ bearing() [2/2]

Rot2 gtsam::Pose2::bearing ( const Pose2 pose,
OptionalJacobian< 1, 3 >  H1 = {},
OptionalJacobian< 1, 3 >  H2 = {} 
) const

Calculate bearing to another pose

Parameters
pointSO(2) location of other pose
Returns
2D rotation $ \in SO(2) $

Definition at line 259 of file Pose2.cpp.

◆ equals()

bool gtsam::Pose2::equals ( const Pose2 pose,
double  tol = 1e-9 
) const

assert equality up to a tolerance

Definition at line 61 of file Pose2.cpp.

◆ Expmap()

Pose2 gtsam::Pose2::Expmap ( const Vector3 xi,
ChartJacobian  H = {} 
)
static

Exponential map at identity - create a rotation from canonical coordinates $ [T_x,T_y,\theta] $.

Definition at line 66 of file Pose2.cpp.

◆ ExpmapDerivative()

Matrix3 gtsam::Pose2::ExpmapDerivative ( const Vector3 v)
static

Derivative of Expmap.

Definition at line 148 of file Pose2.cpp.

◆ Identity()

static Pose2 gtsam::Pose2::Identity ( )
inlinestatic

identity for group operation

Definition at line 125 of file Pose2.h.

◆ inverse()

Pose2 gtsam::Pose2::inverse ( ) const

inverse

Definition at line 201 of file Pose2.cpp.

◆ Logmap()

Vector3 gtsam::Pose2::Logmap ( const Pose2 p,
ChartJacobian  H = {} 
)
static

Log map at identity - return the canonical coordinates $ [T_x,T_y,\theta] $ of this rotation.

Definition at line 82 of file Pose2.cpp.

◆ LogmapDerivative()

Matrix3 gtsam::Pose2::LogmapDerivative ( const Pose2 v)
static

Derivative of Logmap.

Definition at line 179 of file Pose2.cpp.

◆ matrix()

Matrix3 gtsam::Pose2::matrix ( ) const

Definition at line 36 of file Pose2.cpp.

◆ operator*() [1/2]

Pose2 gtsam::Pose2::operator* ( const Pose2 p2) const
inline

compose syntactic sugar

Definition at line 131 of file Pose2.h.

◆ operator*() [2/2]

Point2 gtsam::Pose2::operator* ( const Point2 point) const
inline

syntactic sugar for transformFrom

Definition at line 237 of file Pose2.h.

◆ print()

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

print with optional string

Definition at line 50 of file Pose2.cpp.

◆ r()

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

rotation

Definition at line 258 of file Pose2.h.

◆ range() [1/2]

double gtsam::Pose2::range ( const Point2 point,
OptionalJacobian< 1, 3 >  H1 = {},
OptionalJacobian< 1, 2 >  H2 = {} 
) const

Calculate range to a landmark

Parameters
point2D location of landmark
Returns
range (double)

Definition at line 270 of file Pose2.cpp.

◆ range() [2/2]

double gtsam::Pose2::range ( const Pose2 point,
OptionalJacobian< 1, 3 >  H1 = {},
OptionalJacobian< 1, 3 >  H2 = {} 
) const

Calculate range to another pose

Parameters
point2D location of other pose
Returns
range (double)

Definition at line 287 of file Pose2.cpp.

◆ rotation()

const Rot2& gtsam::Pose2::rotation ( OptionalJacobian< 1, 3 >  Hself = {}) const
inline

rotation

Definition at line 270 of file Pose2.h.

◆ rotationInterval()

static std::pair<size_t, size_t> gtsam::Pose2::rotationInterval ( )
inlinestatic

Return the start and end indices (inclusive) of the rotation component of the exponential map parameterization

Returns
a pair of [start, end] indices into the tangent space vector

Definition at line 328 of file Pose2.h.

◆ t()

const Point2& gtsam::Pose2::t ( ) const
inline

translation

Definition at line 255 of file Pose2.h.

◆ theta()

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

get theta

Definition at line 252 of file Pose2.h.

◆ transformFrom() [1/2]

Point2 gtsam::Pose2::transformFrom ( const Point2 point,
OptionalJacobian< 2, 3 >  Dpose = {},
OptionalJacobian< 2, 2 >  Dpoint = {} 
) const

Return point coordinates in global frame

Definition at line 226 of file Pose2.cpp.

◆ transformFrom() [2/2]

Matrix gtsam::Pose2::transformFrom ( const Matrix points) const

transform many points in Pose coordinates and transform to world.

Parameters
points2*N matrix in Pose coordinates
Returns
points in world coordinates, as 2*N Matrix

Definition at line 236 of file Pose2.cpp.

◆ transformTo() [1/2]

Point2 gtsam::Pose2::transformTo ( const Point2 point,
OptionalJacobian< 2, 3 >  Dpose = {},
OptionalJacobian< 2, 2 >  Dpoint = {} 
) const

Return point coordinates in pose coordinate frame

Definition at line 207 of file Pose2.cpp.

◆ transformTo() [2/2]

Matrix gtsam::Pose2::transformTo ( const Matrix points) const

transform many points in world coordinates and transform to Pose.

Parameters
points2*N matrix in world coordinates
Returns
points in Pose coordinates, as 2*N Matrix

Definition at line 216 of file Pose2.cpp.

◆ translation()

const Point2& gtsam::Pose2::translation ( OptionalJacobian< 2, 3 >  Hself = {}) const
inline

translation

Definition at line 261 of file Pose2.h.

◆ translationInterval()

static std::pair<size_t, size_t> gtsam::Pose2::translationInterval ( )
inlinestatic

Return the start and end indices (inclusive) of the translation component of the exponential map parameterization

Returns
a pair of [start, end] indices into the tangent space vector

Definition at line 321 of file Pose2.h.

◆ wedge()

static Matrix3 gtsam::Pose2::wedge ( double  vx,
double  vy,
double  w 
)
inlinestatic

wedge for SE(2):

Parameters
xi3-dim twist (v,omega) where omega is angular velocity v (vx,vy) = 2D velocity
Returns
xihat, 3*3 element of Lie algebra that can be exponentiated

Definition at line 186 of file Pose2.h.

◆ x()

double gtsam::Pose2::x ( ) const
inline

get x

Definition at line 246 of file Pose2.h.

◆ y()

double gtsam::Pose2::y ( ) const
inline

get y

Definition at line 249 of file Pose2.h.

Friends And Related Function Documentation

◆ operator<<

GTSAM_EXPORT friend std::ostream& operator<< ( std::ostream &  os,
const Pose2 p 
)
friend

Output stream operator.

Definition at line 55 of file Pose2.cpp.

Member Data Documentation

◆ r_

Rot2 gtsam::Pose2::r_
private

Definition at line 49 of file Pose2.h.

◆ t_

Point2 gtsam::Pose2::t_
private

Definition at line 50 of file Pose2.h.


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


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:47:06