Detailed Description

template<int K_, class Derived>
class gtsam::ExtendedPose3< K_, Derived >

Lie group SE_k(3): semidirect product of SO(3) with k copies of R^3.

State ordering is (R, x_1, ..., x_k) with R in SO(3) and x_i in R^3. Tangent ordering is [omega, rho_1, ..., rho_k], each block in R^3.

The manifold dimension is 3+3k and the homogeneous matrix size is 3+k. Template parameter K can be fixed (K >= 1) or Eigen::Dynamic.

Inheritance diagram for gtsam::ExtendedPose3< K_, Derived >:

Access
size_t	k () const
size_t	dim () const
const Rot3 &	rotation (ComponentJacobian H={}) const
	Rotation component.
Point3	x (size_t i, ComponentJacobian H={}) const
	i-th R^3 component, returned by value.
const Matrix3K &	xMatrix () const
	Access all x_i blocks.
Matrix3K &	xMatrix ()
	Mutable access to all x_i blocks.
static size_t	Dimension (size_t k)
	Runtime manifold dimension helper.

Group
This	inverse () const
	Group inverse.
This	operator* (const This &other) const
	Group composition.
template<int K__ = K_, typename = IsFixed<K__>>
static This	Identity ()
	Identity element for fixed-size K.
template<int K__ = K_, typename = IsDynamic<K__>>
static This	Identity (size_t k=0)
	Identity element for dynamic-size K.

Lie Group
Jacobian	AdjointMap () const
	Adjoint map.
static This	Expmap (const TangentVector &xi, ChartJacobian Hxi={})
	Exponential map from tangent to group.
static TangentVector	Logmap (const This &pose, ChartJacobian Hpose={})
	Logarithm map from group to tangent.
static Jacobian	adjointMap (const TangentVector &xi)
	Lie algebra adjoint map.
static Jacobian	ExpmapDerivative (const TangentVector &xi)
	Jacobian of Expmap.
static Jacobian	LogmapDerivative (const TangentVector &xi)
	Jacobian of Logmap evaluated from tangent coordinates.
static Jacobian	LogmapDerivative (const This &pose)
	Jacobian of Logmap evaluated at a group element.

Matrix Lie Group
MatrixRep	matrix () const
	Homogeneous matrix representation.
static LieAlgebra	Hat (const TangentVector &xi)
	Hat operator from tangent to Lie algebra.
static TangentVector	Vee (const LieAlgebra &X)
	Vee operator from Lie algebra to tangent.

Public Member Functions
Constructors
template<int K__ = K_, typename = IsFixed<K__>>
	ExtendedPose3 ()
	Construct a fixed-size identity element.
template<int K__ = K_, typename = IsDynamic<K__>>
	ExtendedPose3 (size_t k=0)
	Construct a dynamic-size identity element.
	ExtendedPose3 (const ExtendedPose3 &)=default
	Copy constructor.
ExtendedPose3 &	operator= (const ExtendedPose3 &)=default
	Copy assignment.
	ExtendedPose3 (const Rot3 &R, const Matrix3K &x)
	Construct from rotation and 3xk block.
template<int FixedK = K, typename = IsFixed<FixedK>, typename... Vecs, typename = std::enable_if_t< sizeof...(Vecs) == FixedK && (std::is_constructible_v<Point3, Vecs> && ...)>>
	ExtendedPose3 (const Rot3 &R, const Vecs &... xs)
	Construct a fixed-size state from rotation and K 3-vectors.
	ExtendedPose3 (const MatrixRep &T)
	Construct from homogeneous matrix representation.
Testable
void	print (const std::string &s="") const
	Print this state.
bool	equals (const ExtendedPose3 &other, double tol=1e-9) const
	Equality check with tolerance.
Public Member Functions inherited from gtsam::MatrixLieGroup< std::conditional_t< std::is_void_v< Derived >, ExtendedPose3< K_, void >, Derived >,(K_==Eigen::Dynamic) ? Eigen::Dynamic :3+3 *K_,(K_==Eigen::Dynamic) ? Eigen::Dynamic :3+K_ >
std::enable_if_t< M !=Eigen::Dynamic, int >	dim () const
	Provided fixed dimension in dim() if needed.
Eigen::Matrix< double, internal::product(N, N), 1 >	vec (OptionalJacobian< internal::product(N, N), D > H={}) const
	Vectorize the matrix representation of a Lie group element.
Jacobian	AdjointMap () const
	A generic implementation of AdjointMap for matrix Lie groups.
TangentVector	Adjoint (const TangentVector &xi, ChartJacobian H_this={}, ChartJacobian H_xi={}) const
	Adjoint action on a tangent vector.
TangentVector	AdjointTranspose (const TangentVector &x, ChartJacobian H_this={}, ChartJacobian H_x={}) const
	Dual Adjoint action on a tangent covector.
Public Member Functions inherited from gtsam::LieGroup< std::conditional_t< std::is_void_v< Derived >, ExtendedPose3< K_, void >, Derived >, D >
SOn	compose (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const
SOn	between (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const
GTSAM_EXPORT SOn	compose (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const
GTSAM_EXPORT SOn	between (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const
std::enable_if_t< M !=Eigen::Dynamic, int >	dim () const
	Provided fixed dimension in dim() if needed.
const std::conditional_t< std::is_void_v< Derived >, ExtendedPose3< K_, void >, Derived > &	derived () const
std::conditional_t< std::is_void_v< Derived >, ExtendedPose3< K_, void >, Derived >	inverse (ChartJacobian H) const
std::conditional_t< std::is_void_v< Derived >, ExtendedPose3< K_, void >, Derived >	expmap (const TangentVector &v) const
	expmap as required by manifold concept Applies exponential map to v and composes with *this
TangentVector	logmap (const std::conditional_t< std::is_void_v< Derived >, ExtendedPose3< K_, void >, Derived > &g) const
	logmap as required by manifold concept Applies logarithmic map to group element that takes *this to g
std::conditional_t< std::is_void_v< Derived >, ExtendedPose3< K_, void >, Derived >	retract (const TangentVector &v) const
	retract as required by manifold concept: applies v at *this
TangentVector	localCoordinates (const std::conditional_t< std::is_void_v< Derived >, ExtendedPose3< K_, void >, Derived > &g) const
	localCoordinates as required by manifold concept: finds tangent vector between *this and g

Static Public Attributes
static constexpr int	K = K_
static constexpr int	dimension
static constexpr int	matrixDim
Static Public Attributes inherited from gtsam::MatrixLieGroup< std::conditional_t< std::is_void_v< Derived >, ExtendedPose3< K_, void >, Derived >,(K_==Eigen::Dynamic) ? Eigen::Dynamic :3+3 *K_,(K_==Eigen::Dynamic) ? Eigen::Dynamic :3+K_ >
static constexpr auto	dimension
Static Public Attributes inherited from gtsam::LieGroup< std::conditional_t< std::is_void_v< Derived >, ExtendedPose3< K_, void >, Derived >, D >
static constexpr auto	dimension

Public Types
using	This
using	Base = MatrixLieGroup<This, dimension, matrixDim>
using	TangentVector = typename Base::TangentVector
using	Jacobian = typename Base::Jacobian
using	ChartJacobian = typename Base::ChartJacobian
using	ComponentJacobian
using	MatrixRep = Eigen::Matrix<double, matrixDim, matrixDim>
	Homogeneous matrix representation in the group.
using	LieAlgebra = Eigen::Matrix<double, matrixDim, matrixDim>
	Lie algebra matrix type used by Hat/Vee.
using	Matrix3K = Eigen::Matrix<double, 3, K>
Public Types inherited from gtsam::MatrixLieGroup< std::conditional_t< std::is_void_v< Derived >, ExtendedPose3< K_, void >, Derived >,(K_==Eigen::Dynamic) ? Eigen::Dynamic :3+3 *K_,(K_==Eigen::Dynamic) ? Eigen::Dynamic :3+K_ >
using	Base
using	ChartJacobian
using	Jacobian
using	TangentVector
using	Vectorized
using	VectorizedJacobian
Public Types inherited from gtsam::LieGroup< std::conditional_t< std::is_void_v< Derived >, ExtendedPose3< K_, void >, Derived >, D >
typedef OptionalJacobian< N, N >	ChartJacobian
typedef Eigen::Matrix< double, N, N >	Jacobian
typedef Eigen::Matrix< double, N, 1 >	TangentVector

Classes
struct	ChartAtOrigin
	Chart operations at identity for LieGroup/Manifold compatibility. More...

Protected Types
template<int K__>
using	IsDynamic = typename std::enable_if<K__ == Eigen::Dynamic, void>::type
template<int K__>
using	IsFixed = typename std::enable_if<K__ >= 1, void>::type

Static Protected Member Functions
static This	MakeReturn (const ExtendedPose3 &value)
static const ExtendedPose3 &	AsBase (const This &value)
static size_t	RuntimeK (const TangentVector &xi)
static void	ZeroJacobian (ChartJacobian H, Eigen::Index d)

Protected Attributes
Rot3	R_
	Rotation component.
Matrix3K	t_
	K translation-like columns in world frame.

Friends
std::ostream &	operator<< (std::ostream &os, const ExtendedPose3 &p)

Additional Inherited Members
Static Public Member Functions inherited from gtsam::MatrixLieGroup< std::conditional_t< std::is_void_v< Derived >, ExtendedPose3< K_, void >, Derived >,(K_==Eigen::Dynamic) ? Eigen::Dynamic :3+3 *K_,(K_==Eigen::Dynamic) ? Eigen::Dynamic :3+K_ >
static constexpr int	Dim ()
	Static method to get the dimension (compile-time or dynamic).
static Jacobian	adjointMap (const TangentVector &xi)
	Lie algebra adjoint map ad_xi, with optional specialization in derived classes.
static TangentVector	adjoint (const TangentVector &xi, const TangentVector &y, ChartJacobian Hxi={}, ChartJacobian H_y={})
	Lie algebra action ad_xi(y), with optional Jacobians.
static TangentVector	adjointTranspose (const TangentVector &xi, const TangentVector &y, ChartJacobian Hxi={}, ChartJacobian H_y={})
	Dual Lie algebra action ad_xi^T(y), with optional Jacobians.
Static Public Member Functions inherited from gtsam::LieGroup< std::conditional_t< std::is_void_v< Derived >, ExtendedPose3< K_, void >, Derived >, D >
static constexpr int	Dim ()
	Static method to get the dimension (compile-time or dynamic).
static std::conditional_t< std::is_void_v< Derived >, ExtendedPose3< K_, void >, Derived >	Retract (const TangentVector &v)
	Retract at origin: possible in Lie group because it has an identity.
static TangentVector	LocalCoordinates (const std::conditional_t< std::is_void_v< Derived >, ExtendedPose3< K_, void >, Derived > &g)
	LocalCoordinates at origin: possible in Lie group because it has an identity.

Member Typedef Documentation

◆ ComponentJacobian

template<int K_, class Derived>

using gtsam::ExtendedPose3< K_, Derived >::ComponentJacobian

Initial value:

 
std::conditional_t<dimension == Eigen::Dynamic,
                   OptionalJacobian<Eigen::Dynamic, Eigen::Dynamic>,
                   OptionalJacobian<3, dimension>>

◆ This

template<int K_, class Derived>

using gtsam::ExtendedPose3< K_, Derived >::This

Initial value:

std::conditional_t<std::is_void_v<Derived>,

ExtendedPose3<K, void>, Derived>

gtsam::ExtendedPose3::ExtendedPose3

ExtendedPose3()

Construct a fixed-size identity element.

Definition ExtendedPose3.h:99

Constructor & Destructor Documentation

◆ ExtendedPose3() [1/5]

template<int K_, class Derived>

template<int K__ = K_, typename = IsFixed<K__>>

gtsam::ExtendedPose3< K_, Derived >::ExtendedPose3 ( )

inline

Construct a fixed-size identity element.

For fixed K, this creates R=I and x_i=0 for i=1..k. The manifold dimension is 3+3k and matrix size is (3+k)x(3+k).

◆ ExtendedPose3() [2/5]

template<int K_, class Derived>

template<int K__ = K_, typename = IsDynamic<K__>>

gtsam::ExtendedPose3< K_, Derived >::ExtendedPose3 ( size_t k = 0 )

inlineexplicit

Construct a dynamic-size identity element.

Parameters

k	Number of R^3 blocks. Creates R=I and x_i=0 for i=1..k. The manifold dimension is 3+3k and matrix size is (3+k)x(3+k).

◆ ExtendedPose3() [3/5]

template<int K, class Derived>

gtsam::ExtendedPose3< K, Derived >::ExtendedPose3	(	const Rot3 &	R,
		const Matrix3K &	x )

Construct from rotation and 3xk block.

Parameters

R	Rotation in SO(3).
x	Matrix in R^(3xk), where column i stores x_i.

◆ ExtendedPose3() [4/5]

template<int K, class Derived>

template<int FixedK, typename, typename... Vecs, typename>

gtsam::ExtendedPose3< K, Derived >::ExtendedPose3	(	const Rot3 &	R,
		const Vecs &...	xs )

Construct a fixed-size state from rotation and K 3-vectors.

The vectors are stored in order as x_1, ..., x_K.

Parameters

R	Rotation in SO(3).
xs	K vector blocks in R^3.

◆ ExtendedPose3() [5/5]

template<int K, class Derived>

gtsam::ExtendedPose3< K, Derived >::ExtendedPose3 ( const MatrixRep & T )

explicit

Construct from homogeneous matrix representation.

Parameters

T	Homogeneous matrix in R^((3+k)x(3+k)). Top-left 3x3 is R, top-right 3xk stores x_1..x_k.

Member Function Documentation

◆ AdjointMap()

template<int K, class Derived>

ExtendedPose3< K, Derived >::Jacobian gtsam::ExtendedPose3< K, Derived >::AdjointMap ( ) const

Adjoint map.

Returns: Matrix in R^(dimxdim).

◆ adjointMap()

template<int K, class Derived>

ExtendedPose3< K, Derived >::Jacobian gtsam::ExtendedPose3< K, Derived >::adjointMap ( const TangentVector & xi )

static

Lie algebra adjoint map.

Parameters

xi	Tangent vector in R^dim.

Returns: ad_xi matrix in R^(dimxdim).

◆ dim()

template<int K_, class Derived>

size_t gtsam::ExtendedPose3< K_, Derived >::dim ( ) const

inline

Returns: Runtime manifold dimension, 3+3k.

◆ Dimension()

template<int K_, class Derived>

size_t gtsam::ExtendedPose3< K_, Derived >::Dimension ( size_t k )

inlinestatic

Runtime manifold dimension helper.

Parameters

k	Number of R^3 blocks.

Returns: 3+3k.

◆ equals()

template<int K, class Derived>

bool gtsam::ExtendedPose3< K, Derived >::equals	(	const ExtendedPose3< K_, Derived > &	other,
		double	tol = 1e-9 ) const

Equality check with tolerance.

Parameters

other	Other state.
tol	Absolute tolerance.

Returns: True if rotation and all x_i blocks are equal within tol.

◆ Expmap()

template<int K, class Derived>

ExtendedPose3< K, Derived >::This gtsam::ExtendedPose3< K, Derived >::Expmap	(	const TangentVector &	xi,
		ChartJacobian	Hxi = {} )

static

Exponential map from tangent to group.

Parameters

xi	Tangent vector in R^dim.
Hxi	Optional Jacobian in R^(dimxdim).

Returns: Group element in SE_k(3).

◆ ExpmapDerivative()

template<int K, class Derived>

ExtendedPose3< K, Derived >::Jacobian gtsam::ExtendedPose3< K, Derived >::ExpmapDerivative ( const TangentVector & xi )

static

Jacobian of Expmap.

Parameters

xi	Tangent vector in R^dim.

Returns: Matrix in R^(dimxdim).

◆ Hat()

template<int K, class Derived>

ExtendedPose3< K, Derived >::LieAlgebra gtsam::ExtendedPose3< K, Derived >::Hat ( const TangentVector & xi )

static

Hat operator from tangent to Lie algebra.

Parameters

xi	Tangent vector in R^dim.

Returns: Matrix in R^((3+k)x(3+k)).

◆ Identity() [1/2]

template<int K_, class Derived>

template<int K__ = K_, typename = IsFixed<K__>>

This gtsam::ExtendedPose3< K_, Derived >::Identity ( )

inlinestatic

Identity element for fixed-size K.

Returns: Identity with manifold dimension 3+3k and matrix size 3+k.

◆ Identity() [2/2]

template<int K_, class Derived>

template<int K__ = K_, typename = IsDynamic<K__>>

This gtsam::ExtendedPose3< K_, Derived >::Identity ( size_t k = 0 )

inlinestatic

Identity element for dynamic-size K.

Parameters

k	Number of R^3 blocks.

Returns: Identity with manifold dimension 3+3k and matrix size 3+k.

◆ inverse()

template<int K, class Derived>

ExtendedPose3< K, Derived >::This gtsam::ExtendedPose3< K, Derived >::inverse ( ) const

Group inverse.

Returns: X^{-1}.

◆ k()

template<int K_, class Derived>

size_t gtsam::ExtendedPose3< K_, Derived >::k ( ) const

inline

Returns: Number of R^3 blocks, k.

◆ Logmap()

template<int K, class Derived>

ExtendedPose3< K, Derived >::TangentVector gtsam::ExtendedPose3< K, Derived >::Logmap	(	const This &	pose,
		ChartJacobian	Hpose = {} )

static

Logarithm map from group to tangent.

Parameters

pose	Group element in SE_k(3).
Hpose	Optional Jacobian in R^(dimxdim).

Returns: Tangent vector in R^dim.

◆ LogmapDerivative() [1/2]

template<int K, class Derived>

ExtendedPose3< K, Derived >::Jacobian gtsam::ExtendedPose3< K, Derived >::LogmapDerivative ( const TangentVector & xi )

static

Jacobian of Logmap evaluated from tangent coordinates.

Parameters

xi	Tangent vector in R^dim.

Returns: Matrix in R^(dimxdim).

◆ LogmapDerivative() [2/2]

template<int K, class Derived>

ExtendedPose3< K, Derived >::Jacobian gtsam::ExtendedPose3< K, Derived >::LogmapDerivative ( const This & pose )

static

Jacobian of Logmap evaluated at a group element.

Parameters

pose	Group element in SE_k(3).

Returns: Matrix in R^(dimxdim).

◆ matrix()

template<int K, class Derived>

ExtendedPose3< K, Derived >::MatrixRep gtsam::ExtendedPose3< K, Derived >::matrix ( ) const

Homogeneous matrix representation.

Returns: Matrix in R^((3+k)x(3+k)).

◆ operator*()

template<int K, class Derived>

ExtendedPose3< K, Derived >::This gtsam::ExtendedPose3< K, Derived >::operator* ( const This & other ) const

Group composition.

Parameters

other Right-hand operand with the same k.

Returns: this * other.

◆ print()

template<int K, class Derived>

void gtsam::ExtendedPose3< K, Derived >::print ( const std::string & s = "" ) const

Print this state.

Parameters

s	Optional prefix string.

◆ rotation()

template<int K, class Derived>

const Rot3 & gtsam::ExtendedPose3< K, Derived >::rotation ( ComponentJacobian H = {} ) const

Rotation component.

Parameters

H	Optional Jacobian in R^(3xdim) for local rotation coordinates.

Returns: Rotation R.

◆ Vee()

template<int K, class Derived>

ExtendedPose3< K, Derived >::TangentVector gtsam::ExtendedPose3< K, Derived >::Vee ( const LieAlgebra & X )

static

Vee operator from Lie algebra to tangent.

Parameters

X	Matrix in R^((3+k)x(3+k)).

Returns: Tangent vector in R^dim.

◆ x()

template<int K, class Derived>

Point3 gtsam::ExtendedPose3< K, Derived >::x	(	size_t	i,
		ComponentJacobian	H = {} ) const

i-th R^3 component, returned by value.

Parameters

i	Zero-based block index in [0, k).
H	Optional Jacobian in R^(3xdim).

Returns: x_i in R^3.

◆ xMatrix() [1/2]

template<int K, class Derived>

ExtendedPose3< K, Derived >::Matrix3K & gtsam::ExtendedPose3< K, Derived >::xMatrix ( )

Mutable access to all x_i blocks.

Returns: Matrix in R^(3xk) with columns x_1..x_k.

◆ xMatrix() [2/2]

template<int K, class Derived>

const ExtendedPose3< K, Derived >::Matrix3K & gtsam::ExtendedPose3< K, Derived >::xMatrix ( ) const

Access all x_i blocks.

Returns: Matrix in R^(3xk) with columns x_1..x_k.

Member Data Documentation

◆ dimension

template<int K_, class Derived>

int gtsam::ExtendedPose3< K_, Derived >::dimension

inlinestaticconstexpr

Initial value:

=

(K == Eigen::Dynamic) ? Eigen::Dynamic : 3 + 3 * K

◆ matrixDim

template<int K_, class Derived>

int gtsam::ExtendedPose3< K_, Derived >::matrixDim

inlinestaticconstexpr

Initial value:

=

(K == Eigen::Dynamic) ? Eigen::Dynamic : 3 + K

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

/tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/geometry/ExtendedPose3.h
/tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/geometry/ExtendedPose3-inl.h

Detailed Description

Access

Group

Lie Group

Matrix Lie Group

Public Member Functions

Static Public Attributes

Public Types

Classes

Protected Types

Static Protected Member Functions

Protected Attributes

Friends

Additional Inherited Members

Member Typedef Documentation

◆ ComponentJacobian

◆ This

Constructor & Destructor Documentation

◆ ExtendedPose3() [1/5]

◆ ExtendedPose3() [2/5]

◆ ExtendedPose3() [3/5]

◆ ExtendedPose3() [4/5]

◆ ExtendedPose3() [5/5]

Member Function Documentation

◆ AdjointMap()

◆ adjointMap()

◆ dim()

◆ Dimension()

◆ equals()

◆ Expmap()

◆ ExpmapDerivative()

◆ Hat()

◆ Identity() [1/2]

◆ Identity() [2/2]

◆ inverse()

◆ k()

◆ Logmap()

◆ LogmapDerivative() [1/2]

◆ LogmapDerivative() [2/2]

◆ matrix()

◆ operator*()

◆ print()

◆ rotation()

◆ Vee()

◆ x()

◆ xMatrix() [1/2]

◆ xMatrix() [2/2]

Member Data Documentation

◆ dimension

◆ matrixDim