Class inertialsim::geometry::ExtendedPose¶

ClassList > inertialsim > geometry > ExtendedPose

Extended pose (attitude, position, and velocity). More...

#include <extended_pose.h>

Inherits the following classes: inertialsim::geometry::MatrixLieGroup

Public Types¶

Type	Name
typedef MatrixLieGroup< ExtendedPose, 5, 9 >	Base

Public Types inherited from inertialsim::geometry::MatrixLieGroup¶

See inertialsim::geometry::MatrixLieGroup

Type	Name
typedef Eigen::Matrix< Scalar, AlgebraDimension, Eigen::Dynamic >	AlgebraArray Matrix of algebra elements (rows = axes, cols = samples).
typedef Eigen::Matrix< Scalar, AlgebraDimension, 1 >	AlgebraElement Algebra element type (vector).
typedef std::vector< GroupElement >	GroupArray Array of group elements.
typedef Eigen::Matrix< Scalar, GroupDimension, GroupDimension >	GroupElement Group element type (square matrix).
typedef Eigen::Matrix< Scalar, AlgebraDimension, AlgebraDimension >	JacobianMatrix Jacobian matrix (algebra to algebra).
typedef std::vector< JacobianMatrix >	JacobianMatrixArray Array of Jacobian matrices.

Public Functions¶

Type	Name
std::tuple< Rotation, Translation, Vector >	AsApv () const Return attitude, position, and velocity.
const Rotation &	AsAttitude () const Return the attitude component.
const Translation &	AsPosition () const Return the position component.
const Vector &	AsVelocity () const Return the velocity component.
std::tuple< Vector3D, Vector3D, Vector3D >	Error (const ExtendedPose & reference) const Calculate error compared to a reference pose.
const Rotation &	attitude () const Get the attitude (rotation) component.
int	num_poses () const Number of extended poses stored.
const Translation &	position () const Get the position (translation) component.
const Vector &	velocity () const Get the velocity component.

Public Functions inherited from inertialsim::geometry::MatrixLieGroup¶

See inertialsim::geometry::MatrixLieGroup

Type	Name
Vector3D	Apply (const Vector3D & vectors) const Apply the group action to an array of vectors.
Derived	Interpolate (const Timestamps & interp_time, Interpolator method=Interpolator::LINEAR) const Interpolate at given times.
Derived	Inverse () const Invert an instance.
Derived	Slice (int start, int end) const Get a slice of elements.
AlgebraArray	TimeDerivative (int order=1, int accuracy=4, const std::string & side="right") const Compute time derivative using finite differences.
int	num_elements () const Number of elements stored in the instance.
Derived	operator* (const Derived & rhs) const Composition operator on the matrix group. Supports broadcasting: if either lhs or rhs has size 1, it will be broadcast against all elements of the other operand. Derived classes may implement more efficient methods.
Derived	operator[] (int index) const Get a single element by index.
const std::optional< Timestamps > &	time () const Time array (or nullopt if not set).
virtual	~MatrixLieGroup () = default

Public Static Functions¶

Type	Name
ExtendedPose	FromApv (const Rotation & attitude, const Translation & position, const Vector & velocity, const std::optional< Timestamps > & time=std::nullopt) Construct from attitude, position, and velocity.
ExtendedPose	FromApv (const RotationMatrixArray & rotation_matrices, const Vector3D & position_xyz, const Vector3D & velocity_xyz, const std::optional< Timestamps > & time=std::nullopt, bool orthonormalize=false) Construct from rotation matrices, position, and velocity vectors.

Public Static Functions inherited from inertialsim::geometry::MatrixLieGroup¶

See inertialsim::geometry::MatrixLieGroup

Type	Name
Derived	Compose (const std::vector< Derived > & instances) Compose a sequence of instances (first to last).
Derived	FromIdentity (int num_elements=1) Create identity elements.
Derived	FromRandom (int num_elements=1, std::optional< Eigen::numext::uint64_t > seed=std::nullopt) Create random elements.

Protected Functions¶

Type	Name
AlgebraArray	AsAlgebraImpl () const
GroupArray	AsGroupImpl () const Return as group elements (for CRTP interface).
	ExtendedPose () Default constructor.
	ExtendedPose (const Rotation & attitude, const Translation & position, const Vector & velocity, const std::optional< Timestamps > & time=std::nullopt) Construct from attitude, position, and velocity.
ExtendedPose	InverseImpl () const Return the inverse extended pose.
int	num_elements_impl () const Number of extended poses stored.

Protected Functions inherited from inertialsim::geometry::MatrixLieGroup¶

See inertialsim::geometry::MatrixLieGroup

Type	Name
AlgebraArray	AsAlgebra () const Get the algebra elements.
GroupArray	AsGroup () const Get the group elements.
Derived	CubicHermiteInterpolate (const Timestamps & time) const Cubic Hermite spline interpolation.
Derived	LinearInterpolate (const Timestamps & time) const Linear interpolation implementation.
	MatrixLieGroup (const std::optional< Timestamps > & time=std::nullopt) Constructor.
Derived &	derived () CRTP helper to access derived class.
const Derived &	derived () const
int	num_times () const

Protected Static Functions¶

Type	Name
GroupElement	ExpImpl (const AlgebraElement & vector) Exponential map: se_2(3) to SE_2(3).
ExtendedPose	FromAlgebraImpl (const AlgebraArray & algebra, const std::optional< Timestamps > & time)
ExtendedPose	FromGroupImpl (const GroupArray & elements, const std::optional< Timestamps > & time) Construct from group elements (for CRTP interface).
ExtendedPose	FromIdentityImpl (int num_elements) Construct identity extended poses.
ExtendedPose	FromRandomImpl (int num_poses, std::optional< uint64_t > seed=std::nullopt) Construct random transforms.
AlgebraElement	LogImpl (const GroupElement & element) Logarithm map: SE_2(3) to se_2(3).

Protected Static Functions inherited from inertialsim::geometry::MatrixLieGroup¶

See inertialsim::geometry::MatrixLieGroup

Type	Name
GroupElement	Exp (const AlgebraElement & vector) Exponential map from algebra to group.
Derived	FromAlgebra (const AlgebraArray & elements, const std::optional< Timestamps > & time) Create instance from algebra elements.
Derived	FromGroup (const GroupArray & elements, const std::optional< Timestamps > & time) Create instance from group elements.
JacobianMatrixArray	Jacobian (const AlgebraArray & algebra, const std::string & side="left", bool inverse=false) Calculate the Jacobian given algebra elements.
AlgebraElement	Log (const GroupElement & element) Logarithm map from group to algebra.

Detailed Description¶

ExtendedPose behaves similarly to Pose with the addition of velocity. Represents elements of the SE_2(3) Lie group.

Public Types Documentation¶

typedef Base¶

using inertialsim::geometry::ExtendedPose::Base =  MatrixLieGroup<ExtendedPose, 5, 9>;

Public Functions Documentation¶

function AsApv¶

Return attitude, position, and velocity.

inline std::tuple< Rotation , Translation , Vector > inertialsim::geometry::ExtendedPose::AsApv () const

Returns:

Tuple of Rotation, Translation, and Vector.

function AsAttitude¶

Return the attitude component.

inline const Rotation & inertialsim::geometry::ExtendedPose::AsAttitude () const

Returns:

Rotation instance.

function AsPosition¶

Return the position component.

inline const Translation & inertialsim::geometry::ExtendedPose::AsPosition () const

Returns:

Translation instance.

function AsVelocity¶

Return the velocity component.

inline const Vector & inertialsim::geometry::ExtendedPose::AsVelocity () const

Returns:

Vector instance.

function Error¶

Calculate error compared to a reference pose.

inline std::tuple< Vector3D, Vector3D, Vector3D > inertialsim::geometry::ExtendedPose::Error (
    const ExtendedPose & reference
) const

Parameters:

reference Reference pose.

Returns:

Tuple of attitude, position, and velocity errors.

function attitude¶

Get the attitude (rotation) component.

inline const Rotation & inertialsim::geometry::ExtendedPose::attitude () const

function num_poses¶

Number of extended poses stored.

inline int inertialsim::geometry::ExtendedPose::num_poses () const

function position¶

Get the position (translation) component.

inline const Translation & inertialsim::geometry::ExtendedPose::position () const

function velocity¶

Get the velocity component.

inline const Vector & inertialsim::geometry::ExtendedPose::velocity () const

Public Static Functions Documentation¶

function FromApv [½]¶

Construct from attitude, position, and velocity.

static inline ExtendedPose inertialsim::geometry::ExtendedPose::FromApv (
    const Rotation & attitude,
    const Translation & position,
    const Vector & velocity,
    const std::optional< Timestamps > & time=std::nullopt
)

Parameters:

attitude Rotation instance.
position Translation instance.
velocity Vector instance.
time Optional time array.

Returns:

ExtendedPose instance.

function FromApv [2/2]¶

Construct from rotation matrices, position, and velocity vectors.

static inline ExtendedPose inertialsim::geometry::ExtendedPose::FromApv (
    const RotationMatrixArray & rotation_matrices,
    const Vector3D & position_xyz,
    const Vector3D & velocity_xyz,
    const std::optional< Timestamps > & time=std::nullopt,
    bool orthonormalize=false
)

Parameters:

rotation_matrices Vector of 3x3 rotation matrices.
position_xyz Position coordinates (3xN).
velocity_xyz Velocity coordinates (3xN).
time Optional time array.
orthonormalize If true, enforce orthonormality.

Returns:

ExtendedPose instance.

Protected Functions Documentation¶

function AsAlgebraImpl¶

inline AlgebraArray inertialsim::geometry::ExtendedPose::AsAlgebraImpl () const

function AsGroupImpl¶

Return as group elements (for CRTP interface).

inline GroupArray inertialsim::geometry::ExtendedPose::AsGroupImpl () const

Returns:

Vector of 5x5 matrices.

function ExtendedPose [½]¶

Default constructor.

inline inertialsim::geometry::ExtendedPose::ExtendedPose ()

function ExtendedPose [2/2]¶

Construct from attitude, position, and velocity.

inline inertialsim::geometry::ExtendedPose::ExtendedPose (
    const Rotation & attitude,
    const Translation & position,
    const Vector & velocity,
    const std::optional< Timestamps > & time=std::nullopt
)

Parameters:

attitude Rotation component.
position Translation component.
velocity Vector component.
time Optional time array.

function InverseImpl¶

Return the inverse extended pose.

inline ExtendedPose inertialsim::geometry::ExtendedPose::InverseImpl () const

Returns:

ExtendedPose with inverted transformation.

function num_elements_impl¶

Number of extended poses stored.

inline int inertialsim::geometry::ExtendedPose::num_elements_impl () const

Protected Static Functions Documentation¶

function ExpImpl¶

Exponential map: se_2(3) to SE_2(3).

static inline GroupElement inertialsim::geometry::ExtendedPose::ExpImpl (
    const AlgebraElement & vector
)

Parameters:

vector 9D algebra element [rotation_vector, position, velocity].

Returns:

5x5 extended homogeneous transform matrix.

function FromAlgebraImpl¶

static inline ExtendedPose inertialsim::geometry::ExtendedPose::FromAlgebraImpl (
    const AlgebraArray & algebra,
    const std::optional< Timestamps > & time
)

function FromGroupImpl¶

Construct from group elements (for CRTP interface).

static inline ExtendedPose inertialsim::geometry::ExtendedPose::FromGroupImpl (
    const GroupArray & elements,
    const std::optional< Timestamps > & time
)

Parameters:

elements Array of 5x5 extended homogeneous transform matrices.
time Optional time array.

Returns:

ExtendedPose instance.

function FromIdentityImpl¶

Construct identity extended poses.

static inline ExtendedPose inertialsim::geometry::ExtendedPose::FromIdentityImpl (
    int num_elements
)

Parameters:

num_elements Number of identity poses.

Returns:

ExtendedPose instance.

function FromRandomImpl¶

Construct random transforms.

static inline ExtendedPose inertialsim::geometry::ExtendedPose::FromRandomImpl (
    int num_poses,
    std::optional< uint64_t > seed=std::nullopt
)

Parameters:

num_poses Number of random poses.
seed Optional random seed.

Returns:

ExtendedPose instance with random poses.

function LogImpl¶

Logarithm map: SE_2(3) to se_2(3).

static inline AlgebraElement inertialsim::geometry::ExtendedPose::LogImpl (
    const GroupElement & element
)

Parameters:

element 5x5 extended homogeneous transform matrix.

Returns:

9D algebra element [rotation_vector, position, velocity].

The documentation for this class was generated from the following file cpp/include/inertialsim/geometry/extended_pose.h