#include <Pose2.h>

Inheritance diagram for gtsam::Pose2:

Collaboration diagram for gtsam::Pose2:

Classes
struct	ChartAtOrigin

Public Types
typedef Rot2	Rotation

typedef Point2	Translation

enum

typedef OptionalJacobian< N, N >	ChartJacobian

typedef Eigen::Matrix< double, N, N >	Jacobian

typedef Eigen::Matrix< double, N, 1 >	TangentVector

Public Member Functions
const Pose2 &	derived () const

Pose2	compose (const Pose2 &g) const

Pose2	compose (const Pose2 &g, ChartJacobian H1, ChartJacobian H2={}) const

GTSAM_EXPORT SOn	compose (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const

Pose2	between (const Pose2 &g) const

Pose2	between (const Pose2 &g, ChartJacobian H1, ChartJacobian H2={}) const

GTSAM_EXPORT SOn	between (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const

Pose2	inverse (ChartJacobian H) const

Pose2	expmap (const TangentVector &v) const

Pose2	expmap (const TangentVector &v, ChartJacobian H1, ChartJacobian H2={}) const
	expmap with optional derivatives

TangentVector	logmap (const Pose2 &g) const

TangentVector	logmap (const Pose2 &g, ChartJacobian H1, ChartJacobian H2={}) const
	logmap with optional derivatives

Pose2	retract (const TangentVector &v) const
	retract as required by manifold concept: applies v at *this

Pose2	retract (const TangentVector &v, ChartJacobian H1, ChartJacobian H2={}) const
	retract with optional derivatives

TangentVector	localCoordinates (const Pose2 &g) const
	localCoordinates as required by manifold concept: finds tangent vector between *this and g

TangentVector	localCoordinates (const Pose2 &g, ChartJacobian H1, ChartJacobian H2={}) const
	localCoordinates with optional derivatives

Static Public Member Functions
static Pose2	Retract (const TangentVector &v)
	Retract at origin: possible in Lie group because it has an identity.

static Pose2	Retract (const TangentVector &v, ChartJacobian H)
	Retract at origin with optional derivative.

static TangentVector	LocalCoordinates (const Pose2 &g)
	LocalCoordinates at origin: possible in Lie group because it has an identity.

static TangentVector	LocalCoordinates (const Pose2 &g, ChartJacobian H)
	LocalCoordinates at origin with optional derivative.

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< Pose2 >	Align (const Point2Pairs &abPointPairs)

static std::optional< Pose2 >	Align (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

Pose2	operator* (const Pose2 &p2) const
	compose syntactic sugar

static Pose2	Identity ()
	identity for group operation

Lie Group
GTSAM_EXPORT Matrix3	AdjointMap () const

Vector3	Adjoint (const Vector3 &xi) const
	Apply AdjointMap to twist xi.

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] \).

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.

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.

static GTSAM_EXPORT Matrix3	LogmapDerivative (const Pose2 &v)
	Derivative of Logmap.

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

double	y () const
	get y

double	theta () const
	get theta

const Point2 &	t () const
	translation

const Rot2 &	r () const
	rotation

const Point2 &	translation () const
	translation

const Rot2 &	rotation () const
	rotation

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_t >	translationInterval ()

static std::pair< size_t, size_t >	rotationInterval ()

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

Detailed Description

A 2D pose (Point2,Rot2)

Member Typedef Documentation

◆ Rotation

typedef Rot2 gtsam::Pose2::Rotation

Pose Concept requirements

Constructor & Destructor Documentation

◆ Pose2() [1/7]

gtsam::Pose2::Pose2 ( )

inline

default constructor = origin

◆ Pose2() [2/7]

gtsam::Pose2::Pose2 ( const Pose2 & pose )

inline

copy constructor

◆ Pose2() [3/7]

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

inline

construct from (x,y,theta)

Parameters

x	x coordinate
y	y coordinate
theta	angle with positive X-axis

◆ Pose2() [4/7]

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

inline

construct from rotation and translation

◆ Pose2() [5/7]

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

inline

construct from r,t

◆ Pose2() [6/7]

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

inline

Constructor from 3*3 matrix

◆ Pose2() [7/7]

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

inline

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

Member Function Documentation

◆ 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

◆ AdjointMap()

GTSAM_EXPORT 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)

◆ adjointMap()

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

static

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

◆ 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.

◆ Align()

static 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.

◆ bearing() [1/2]

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

Calculate bearing to a landmark

Parameters

point 2D location of landmark

Returns: 2D rotation \( \in SO(2) \)

◆ bearing() [2/2]

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

Calculate bearing to another pose

Parameters

point SO(2) location of other pose

Returns: 2D rotation \( \in SO(2) \)

◆ equals()

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

assert equality up to a tolerance

◆ expmap()

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

inlineinherited

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

◆ logmap()

TangentVector gtsam::LieGroup< Pose2 , N >::logmap ( const Pose2 & g ) const

inlineinherited

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

◆ operator*()

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

inline

syntactic sugar for transformFrom

◆ print()

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

print with optional string

◆ range() [1/2]

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

Calculate range to a landmark

Parameters

point 2D location of landmark

Returns: range (double)

◆ range() [2/2]

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

Calculate range to another pose

Parameters

point 2D location of other pose

Returns: range (double)

◆ 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

◆ transformFrom() [1/2]

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

Return point coordinates in global frame

◆ transformFrom() [2/2]

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

transform many points in Pose coordinates and transform to world.

Parameters

points 2*N matrix in Pose coordinates

Returns: points in world coordinates, as 2*N Matrix

◆ transformTo() [1/2]

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

Return point coordinates in pose coordinate frame

◆ transformTo() [2/2]

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

transform many points in world coordinates and transform to Pose.

Parameters

points 2*N matrix in world coordinates

Returns: points in Pose coordinates, as 2*N Matrix

◆ 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

◆ wedge()

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

inlinestatic

wedge for SE(2):

Parameters

xi	3-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

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

/home/docs/checkouts/readthedocs.org/user_builds/gtsam-jlblanco-docs/checkouts/latest/gtsam/geometry/Pose2.h

Classes

Public Types

Public Member Functions

Static Public Member Functions

Standard Constructors

Advanced Constructors

Testable

Group

Lie Group

Group Action on Point2

Standard Interface

Advanced Interface

Detailed Description

Member Typedef Documentation

◆ Rotation

Constructor & Destructor Documentation

◆ Pose2() [1/7]

◆ Pose2() [2/7]

◆ Pose2() [3/7]

◆ Pose2() [4/7]

◆ Pose2() [5/7]

◆ Pose2() [6/7]

◆ Pose2() [7/7]

Member Function Documentation

◆ adjoint()

◆ AdjointMap()

◆ adjointMap()

◆ adjointTranspose()

◆ Align()

◆ bearing() [1/2]

◆ bearing() [2/2]

◆ equals()

◆ expmap()

◆ logmap()

◆ operator*()

◆ print()

◆ range() [1/2]

◆ range() [2/2]

◆ rotationInterval()

◆ transformFrom() [1/2]

◆ transformFrom() [2/2]

◆ transformTo() [1/2]

◆ transformTo() [2/2]

◆ translationInterval()

◆ wedge()