#include <Pose2.h>

Inheritance diagram for gtsam::Pose2:

Classes
struct	ChartAtOrigin

Public Types
using	LieAlgebra = Matrix3
	LieGroup Concept requirements. More...

using	Rotation = Rot2
	Pose Concept requirements. More...

using	Translation = Point2

Public Types inherited from gtsam::LieGroup< Pose2, 3 >
typedef OptionalJacobian< N, N >	ChartJacobian

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

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

Private Attributes
Rot2	r_

Point2	t_

Standard Constructors
	Pose2 ()

	Pose2 (const Pose2 &pose)=default

Pose2 &	operator= (const Pose2 &other)=default

	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
void	print (const std::string &s="") const

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

Group
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
Matrix3	AdjointMap () const

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

static Pose2	Expmap (const Vector3 &xi, ChartJacobian H={})
	Exponential map at identity - create a rotation from canonical coordinates $[T_x,T_y,\theta]$ . More...

static 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 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	ExpmapDerivative (const Vector3 &v)
	Derivative of Expmap. More...

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

static Matrix3	Hat (const Vector3 &xi)
	Hat maps from tangent vector to Lie algebra. More...

static Vector3	Vee (const Matrix3 &X)
	Vee maps from Lie algebra to tangent vector. More...

Group Action on Point2
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...

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 Point2 &	t () const
	translation More...

const Rot2 &	r () const
	rotation More...

const Point2 &	translation (OptionalJacobian< 2, 3 > Hself={}) const
	translation More...

const Rot2 &	rotation (OptionalJacobian< 1, 3 > Hself={}) const
	rotation More...

Matrix3	matrix () const

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

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

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

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

Advanced Interface
Vector9	vec (OptionalJacobian< 9, 3 > H={}) const
	Return vectorized SE(2) matrix in column order. More...

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. 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 Pose2 &	derived () 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...

Static Public Attributes inherited from gtsam::LieGroup< Pose2, 3 >
constexpr static auto	dimension

Detailed Description

A 2D pose (Point2,Rot2)

Definition at line 39 of file Pose2.h.

Member Typedef Documentation

◆ LieAlgebra

using gtsam::Pose2::LieAlgebra = Matrix3

LieGroup Concept requirements.

Definition at line 48 of file Pose2.h.

◆ Rotation

using gtsam::Pose2::Rotation = Rot2

Pose Concept requirements.

Definition at line 44 of file Pose2.h.

◆ Translation

using gtsam::Pose2::Translation = Point2

Definition at line 45 of file Pose2.h.

Constructor & Destructor Documentation

◆ Pose2() [1/7]

gtsam::Pose2::Pose2 ( )

inline

default constructor = origin

Definition at line 61 of file Pose2.h.

◆ Pose2() [2/7]

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

default

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

Definition at line 77 of file Pose2.h.

◆ Pose2() [4/7]

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

inline

construct from rotation and translation

Definition at line 82 of file Pose2.h.

◆ Pose2() [5/7]

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

inline

construct from r,t

Definition at line 87 of file Pose2.h.

◆ Pose2() [6/7]

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

inline

Constructor from 3*3 matrix

Definition at line 90 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 104 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 162 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 174 of file Pose2.h.

◆ adjoint_()

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

inlinestatic

Definition at line 187 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 127 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 138 of file Pose2.cpp.

◆ adjointMap_()

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

inlinestatic

Definition at line 186 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 181 of file Pose2.h.

◆ Align() [1/2]

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

static

Definition at line 403 of file Pose2.cpp.

◆ Align() [2/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 371 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

point 2D location of landmark

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

Definition at line 260 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

point SO(2) location of other pose

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

Definition at line 274 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 62 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 67 of file Pose2.cpp.

◆ ExpmapDerivative()

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

static

Derivative of Expmap.

Definition at line 149 of file Pose2.cpp.

◆ Hat()

Matrix3 gtsam::Pose2::Hat ( const Vector3 & xi )

static

Hat maps from tangent vector to Lie algebra.

Definition at line 207 of file Pose2.cpp.

◆ Identity()

static Pose2 gtsam::Pose2::Identity ( )

inlinestatic

identity for group operation

Definition at line 135 of file Pose2.h.

◆ inverse()

Pose2 gtsam::Pose2::inverse ( ) const

inverse

Definition at line 202 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 83 of file Pose2.cpp.

◆ LogmapDerivative()

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

static

Derivative of Logmap.

Definition at line 180 of file Pose2.cpp.

◆ matrix()

Matrix3 gtsam::Pose2::matrix ( ) const

Definition at line 37 of file Pose2.cpp.

◆ operator*() [1/2]

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

inline

syntactic sugar for transformFrom

Definition at line 238 of file Pose2.h.

◆ operator*() [2/2]

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

inline

compose syntactic sugar

Definition at line 141 of file Pose2.h.

◆ operator=()

Pose2& gtsam::Pose2::operator= ( const Pose2 & other )

default

◆ print()

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

print with optional string

Definition at line 51 of file Pose2.cpp.

◆ r()

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

inline

rotation

Definition at line 259 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

point 2D location of landmark

Returns: range (double)

Definition at line 285 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

point 2D location of other pose

Returns: range (double)

Definition at line 302 of file Pose2.cpp.

◆ rotation()

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

inline

rotation

Definition at line 271 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 329 of file Pose2.h.

◆ t()

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

inline

translation

Definition at line 256 of file Pose2.h.

◆ theta()

double gtsam::Pose2::theta ( ) const

inline

get theta

Definition at line 253 of file Pose2.h.

◆ transformFrom() [1/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

Definition at line 251 of file Pose2.cpp.

◆ transformFrom() [2/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 241 of file Pose2.cpp.

◆ transformTo() [1/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

Definition at line 231 of file Pose2.cpp.

◆ transformTo() [2/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 222 of file Pose2.cpp.

◆ translation()

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

inline

translation

Definition at line 262 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 322 of file Pose2.h.

◆ vec()

Vector9 gtsam::Pose2::vec ( OptionalJacobian< 9, 3 > H = {} ) const

Return vectorized SE(2) matrix in column order.

Definition at line 337 of file Pose2.cpp.

◆ Vee()

Pose2::TangentVector gtsam::Pose2::Vee ( const Matrix3 & X )

static

Vee maps from Lie algebra to tangent vector.

Definition at line 216 of file Pose2.cpp.

◆ x()

double gtsam::Pose2::x ( ) const

inline

get x

Definition at line 247 of file Pose2.h.

◆ y()

double gtsam::Pose2::y ( ) const

inline

get y

Definition at line 250 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 56 of file Pose2.cpp.

Member Data Documentation

◆ r_

Rot2 gtsam::Pose2::r_

private

Definition at line 52 of file Pose2.h.

◆ t_

Point2 gtsam::Pose2::t_

private

Definition at line 53 of file Pose2.h.

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

Classes

Public Types

Private Attributes

Standard Constructors

Advanced Constructors

Testable

Group

Lie Group

Group Action on Point2

Standard Interface

Advanced Interface

Additional Inherited Members

Detailed Description

Member Typedef Documentation

◆ LieAlgebra

◆ Rotation

◆ Translation

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

◆ adjoint()

◆ adjoint_()

◆ AdjointMap()

◆ adjointMap()

◆ adjointMap_()

◆ adjointTranspose()

◆ Align() [1/2]

◆ Align() [2/2]

◆ bearing() [1/2]

◆ bearing() [2/2]

◆ equals()

◆ Expmap()

◆ ExpmapDerivative()

◆ Hat()

◆ Identity()

◆ inverse()

◆ Logmap()

◆ LogmapDerivative()

◆ matrix()

◆ operator*() [1/2]

◆ operator*() [2/2]

◆ operator=()

◆ print()

◆ r()

◆ range() [1/2]

◆ range() [2/2]

◆ rotation()

◆ rotationInterval()

◆ t()

◆ theta()

◆ transformFrom() [1/2]

◆ transformFrom() [2/2]

◆ transformTo() [1/2]

◆ transformTo() [2/2]

◆ translation()

◆ translationInterval()

◆ vec()

◆ Vee()

◆ x()

◆ y()

Friends And Related Function Documentation

◆ operator<<

Member Data Documentation

◆ r_

◆ t_