Containers

struct TransformContainerException : public motion3d::MessageException: Exception thrown by TransformContainer in case motions and poses are mixed.

class TransformContainer

Container class to store, process and convert stamped and unstamped transformations for either motions or poses.

This container makes it possible to store either stamped and unstamped data and enables element access without requiring the user to know, if the data is really stamped or unstamped. Further, the container provides unstamped- or stamped-specific functions, e.g., for searching elements at specific stamps. An invididual transform of this container is referred to as $T_i$ . Chaining two transforms $T_\mathrm{a}$ and $T_\mathrm{b}$ is denoted as $T_\mathrm{a} \circ T_\mathrm{b}$ .

In case stamped functions are called on unstamped data or vice versa, or in case motions and poses are mixed, a TransformContainerException is thrown.

Public Types

using StampType = Time 

using DataType = TransformInterface::Ptr

using DataTypeConst = TransformInterface::ConstPtr

using VectorType = std::vector<DataType>

using MapType = ExtendedMap<StampType, DataType>

using const_stamp_iterator = typename MapType::const_key_iterator

using item_iterator = typename MapType::iterator

Public Functions

DEFINE_POINTERS(TransformContainer)

TransformContainer(bool has_stamps, bool has_poses)

Constructors.

Constructs and initializes an empty TransformContainer.

template<class T> TransformContainer(const std::vector<T> &transforms, bool has_poses)

Constructs and initializes a TransformContainer from unstamped transforms.

Throws:: std::invalid_argument – if null pointers are added.

template<class T> TransformContainer(const std::map<StampType, T> &transforms, bool has_poses)

Constructs and initializes a TransformContainer from stamped transforms.

Throws:: std::invalid_argument – if null pointers are added.

template<class T> TransformContainer(const std::vector<StampType> &stamps, const std::vector<T> &transforms, bool has_poses, bool sorted_data = false)

Constructs and initializes a TransformContainer from separate stamps and transforms.

Throws:

std::invalid_argument – if the sizes of stamps and data are not equal.
std::invalid_argument – if null pointers are added.

TransformContainer(const TransformContainer &other): Copy constructor to copy all transforms.

TransformContainer &operator=(const TransformContainer &other) = delete: Delete copy assignment operator.

TransformContainer(TransformContainer &&other) = default: Default move constructor.

TransformContainer &operator=(TransformContainer &&other) = default: Default move assignment operator.

virtual ~TransformContainer() = default: Default destructor.

inline bool hasStamps() const

Data Access.

Returns:: true if the container has stamped data.

inline bool hasPoses() const

Returns:: true if the container has poses.

inline bool hasMotions() const

Returns:: true if the container has motions.

std::size_t size() const

Returns:: the number of elements.

bool empty() const

Returns:: true if the container has no elements.

inline bool hasStamp(const StampType &stamp) const

Throws:: TransformContainerException – if the data is unstamped.
Returns:: true if the container contains the given stamp.

DataType &at(const std::size_t &index)

Throws:: std::out_of_range – if index is out of range.
Returns:: the element at index.

DataTypeConst at(const std::size_t &index) const

See also

at(const std::size_t&)

DataType &at_stamp(const StampType &stamp)

Throws:

TransformContainerException – if the data is unstamped.
std::out_of_range – if stamp does not exist.

Returns:

the element at stamp.

DataTypeConst at_stamp(const StampType &stamp) const

See also

at_stamp(const StampType&)

StampType stamp_at(const std::size_t &index) const

Throws:

TransformContainerException – if the data is unstamped.
std::out_of_range – if index is out of range.

Returns:

the stamp for the element at at index.

std::pair<const StampType, DataType> &item_at(const std::size_t &index)

Throws:

TransformContainerException – if the data is unstamped.
std::out_of_range – if index is out of range.

Returns:

the pair of stamp and element at index.

std::pair<const StampType, DataTypeConst> item_at(const std::size_t &index) const

See also

item_at(const std::size_t&)

void append(const DataTypeConst &transform)

Appends a new element.

Throws:: TransformContainerException – if the data is stamped.

void append(const StampType &stamp, const DataTypeConst &transform)

Inserts a new element with stamp with the end as hint.

This is more efficient than insert(const StampType&, const DataTypeConst&, const bool), when stamp is larger than all previously inserted timestamps and the data is really inserted at the end. However, it is is less efficient when the data is inserted somewhere else. Existing stamps are not overwritten.

Throws:: TransformContainerException – if the data is unstamped.

void insert(const std::size_t &index, const DataTypeConst &transform)

Inserts a new element at index.

Throws:: TransformContainerException – if the data is stamped.

bool insert(const StampType &stamp, const DataTypeConst &transform, bool overwrite = false)

Inserts a new element at stamp.

Use append(const StampType&, const DataTypeConst&) if the the new stamp is larger than all previously inserted timestamps.

Throws:: TransformContainerException – if the data is unstamped.
Returns:: if the data was inserted.

template<class T> void extend(const std::vector<T> &other)

Appends unstamped data other to *this.

Throws:: TransformContainerException – if the data is stamped.

template<class T> void extend(const std::map<StampType, T> &other, bool overwrite = false)

Inserts stamped data other to *this.

Throws:: TransformContainerException – if the data is unstamped.

void extend(const TransformContainer &other, bool overwrite = false)

Extends *this with transforms from other.

Throws:: TransformContainerException – if hasStamps() of *this and other do not match or if motions and poses are mixed.

inline void extend(const TransformContainer::ConstPtr &other, bool overwrite = false)

See also

extend(const TransformContainer&, bool)

void erase(const std::size_t &index): Removes the element at index.

void erase(const StampType &stamp)

Removes the element at stamp.

Throws:: TransformContainerException – if the data is unstamped.

void erase(iterator position)

Removes the element corresponding to the position iterator.

Throws:: TransformContainerException – if the data is unstamped.

void clear(): Removes all elements.

iterator begin()

Iterator Access.

Returns:: an iterator to the first element.

const_iterator cbegin() const

See also

begin()

inline const_iterator begin() const

See also

begin()

iterator end()

Returns:: the past-the-end iterator corresponding to begin().

const_iterator cend() const

See also

end()

inline const_iterator end() const

See also

end()

const_stamp_iterator cbegin_stamps() const

Throws:: TransformContainerException – if the data is unstamped.
Returns:: an iterator to the first stamp.

inline const_stamp_iterator begin_stamps() const

See also

cbegin_stamps()

const_stamp_iterator cend_stamps() const

Throws:: TransformContainerException – if the data is unstamped.
Returns:: the past-the-end iterator corresponding to cbegin_stamps().

inline const_stamp_iterator end_stamps() const

See also

cend_stamps()

item_iterator begin_items()

Throws:: TransformContainerException – if the data is unstamped.
Returns:: an iterator to the first pair of stamp and element.

const_item_iterator cbegin_items() const

See also

begin_items()

inline const_item_iterator begin_items() const

See also

begin_items()

item_iterator end_items()

Throws:: TransformContainerException – if the data is unstamped.
Returns:: the past-the-end iterator corresponding to begin_items().

const_item_iterator cend_items() const

See also

end_items()

inline const_item_iterator end_items() const

See also

end_items()

item_iterator find_eq(const StampType &stamp)

Find Methods.

Throws:: TransformContainerException – if the data is unstamped.
Returns:: an iterator to the element at stamp or the end() iterator if no element was found.

const_item_iterator cfind_eq(const StampType &stamp) const

See also

find_eq(const StampType&)

inline const_item_iterator find_eq(const StampType &stamp) const

See also

find_eq(const StampType&)

item_iterator find_ge(const StampType &stamp)

Throws:: TransformContainerException – if the data is unstamped.
Returns:: an iterator to the first element with time >= stamp or the end() iterator if no element was found.

const_item_iterator cfind_ge(const StampType &stamp) const

See also

find_ge(const StampType&)

inline const_item_iterator find_ge(const StampType &stamp) const

See also

find_ge(const StampType&)

item_iterator find_gt(const StampType &stamp)

Throws:: TransformContainerException – if the data is unstamped.
Returns:: an iterator to the first element with time > stamp or the end() iterator if no element was found.

const_item_iterator cfind_gt(const StampType &stamp) const

See also

find_gt(const StampType&)

inline const_item_iterator find_gt(const StampType &stamp) const

See also

find_gt(const StampType&)

item_iterator find_le(const StampType &stamp)

Throws:: TransformContainerException – if the data is unstamped.
Returns:: an iterator to the last element with time <= stamp or the end() iterator if no element was found.

const_item_iterator cfind_le(const StampType &stamp) const

See also

find_le(const StampType&)

inline const_item_iterator find_le(const StampType &stamp) const

See also

find_le(const StampType&)

item_iterator find_lt(const StampType &stamp)

Throws:: TransformContainerException – if the data is unstamped.
Returns:: an iterator to the last element with time < stamp or the end() iterator if no element was found.

const_item_iterator cfind_lt(const StampType &stamp) const

See also

find_lt(const StampType&)

inline const_item_iterator find_lt(const StampType &stamp) const

See also

find_lt(const StampType&)

item_iterator find_closest(const StampType &stamp)

Throws:: TransformContainerException – if the data is unstamped.
Returns:: an iterator to the closest element to stamp or the end() iterator if the container is empty.

const_item_iterator cfind_closest(const StampType &stamp) const

See also

find_closest(const StampType&)

inline const_item_iterator find_closest(const StampType &stamp) const

See also

find_closest(const StampType&)

TransformContainer &removeStamps_()

Transformations.

Inplace Implementation.

Inplace variant of removeStamps().

TransformContainer removeStamps() const

Copy Implementation.

Returns:: a TransformContainer with unstamped transforms.

TransformContainer &addStamps_(const std::vector<StampType> &stamps): Inplace variant of addStamps().

TransformContainer addStamps(const std::vector<StampType> &stamps) const

Throws:: TransformContainerException – if the container already has stamps or if the number of stamps does not match the number of transforms.
Returns:: a TransformContainer with stamps added to the transforms.

template<class TransformClass> TransformContainer &asType_(): Inplace variant of asType() const.

template<class TransformClass> TransformContainer asType() const

Template Parameters:: TransformClass – the target transform class
Returns:: a TransformContainer with all transforms converted to the given TransformClass.

TransformContainer &asType_(const TransformType &type): Inplace variant of asType(const TransformType&) const.

TransformContainer asType(const TransformType &type) const

Returns:: a TransformContainer with all transforms converted to the given transform type.

TransformContainer &asPoses_(): Inplace variant of asPoses() const.

TransformContainer asPoses() const

See also

asPoses(const DataTypeConst&) const

Throws:

TransformContainerException – for stamped data, since the timestamp for the additional transform would be unknown.

Returns:

a TransformContainer with poses for unstamped data.

If *this is empty, an empty container is returned.
If *this contains $n$ motions, all motions are converted to $n+1$ poses and the identity transform is used as first pose.
If *this contains poses, the initial pose is retained.

TransformContainer &asPoses_(const DataTypeConst &initial_pose): Inplace variant of asPoses(const DataTypeConst&) const.

TransformContainer asPoses(const DataTypeConst &initial_pose) const

Throws:

TransformContainerException – for stamped data, since the timestamp for the additional transform would be unknown.

Returns:

a TransformContainer with poses and initial_pose as first pose for unstamped data.

If *this is empty, an empty container is returned.
If *this contains $n$ motions, all motions are converted to $n+1$ poses and initial_pose is used as first pose.
If *this contains poses, all transforms are adjusted so the first pose is initial_pose.

TransformContainer &asMotions_(): Inplace variant of asMotions().

TransformContainer asMotions() const

Throws:

TransformContainerException – for stamped data, since it would be unclear, which timestamps should be retained.

Returns:

a TransformContainer with motions for unstamped data.

If *this is empty or contains only a single pose, an empty container is returned.
If *this contains $n$ poses, $n-1$ motions are returned.
If *this contains motions, nothing is changed.

TransformContainer &inverse_(): Inplace variant of inverse().

TransformContainer inverse() const: Inverts all transforms.

TransformContainer &normalized_(): Inplace variant of normalized().

TransformContainer normalized() const: Normalizes all transforms.

TransformContainer &scaleTranslation_(double factor): Inplace variant of scaleTranslation().

TransformContainer scaleTranslation(double factor) const: Scales translation of all transforms by factor.

template<class T> TransformContainer &applyPre_(const T &transform): Inplace variant of applyPre().

template<class T> TransformContainer applyPre(const T &transform) const

Applies transform $T_\mathrm{pre}$ before each transform $T_{i}$ of *this:

$T_\mathrm{pre} \circ T_{i}$

template<class T> TransformContainer &applyPost_(const T &transform): Inplace variant of applyPost().

template<class T> TransformContainer applyPost(const T &transform) const

Applies transform $T_\mathrm{post}$ after each transform $T_{i}$ of *this:

$T_{i} \circ T_\mathrm{post}$

template<class T1, class T2> TransformContainer &apply_(const T1 &transform_pre, const T2 &transform_post): Inplace variant of apply().

template<class T1, class T2> TransformContainer apply(const T1 &transform_pre, const T2 &transform_post) const

Applies transform_pre $T_\mathrm{pre}$ and transform_post $T_\mathrm{post}$ before and after each transform $T_{i}$ of *this, respectively:

$T_\mathrm{pre} \circ T_{i} \circ T_\mathrm{post}$

TransformContainer &applyFunc_(const std::function<DataType(const DataType&)> &func): Inplace variant of applyFunc().

TransformContainer applyFunc(const std::function<DataType(const DataTypeConst&)> &func) const: Applies func on each transform.

TransformContainer &applyIndexFunc_(const std::function<DataType(std::size_t, const DataType&)> &func): Inplace variant of applyIndexFunc().

TransformContainer applyIndexFunc(const std::function<DataType(std::size_t, const DataTypeConst&)> &func) const: Applies func on each transform.

TransformContainer &applyStampFunc_(const std::function<DataType(const StampType&, const DataType&)> &func): Inplace variant of applyStampFunc().

TransformContainer applyStampFunc(const std::function<DataType(const StampType&, const DataTypeConst&)> &func) const

Applies func on each transform.

Throws:: TransformContainerException – for unstamped data

template<class T> TransformContainer &changeFrame_(const T &transform): Inplace variant of changeFrame().

template<class T> TransformContainer changeFrame(const T &transform) const

Changes the coordinate frame of the transforms using the given frame-forward transform $T_\mathrm{frame}$ .

$T_\mathrm{frame}^{-1} \circ T_{i} \circ T_\mathrm{frame}$

template<class TransformClass> std::vector<TransformClass> toVector() const

Conversions.

Template Parameters:: TransformClass – the target transform class
Returns:: a vector with all transforms converted to the given TransformClass.

std::vector<DataType> toVector() const

Returns:: a vector with copies of all transforms in their original transform type.

std::vector<DataType> toVector(const TransformType &type) const

Returns:: a vector with all transforms converted to the given transform type.

template<class TransformClass> Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> toEigenVector() const

Template Parameters:: TransformClass – the target transform class
Returns:: an Eigen vector of all transforms converted to the given TransformClass where each row represents a single transformation.

Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> toEigenVector(const TransformType &type) const

Returns:: an Eigen vector of all transforms converted to the given transform type where each row represents a single transformation.

inline std::string desc() const

Returns:: a description of *this.

Private Functions

void motionsToPoses(const DataTypeConst &initial_pose)

Converts motions to poses inplace, using initial_pose as first pose.

Helper.

If initial_pose is a nullptr, the identity transform is used as first pose.

Throws:: TransformContainerException – for stamped data, since the timestamp for the additional transform would be unknown.

inline VectorType &vector()

inline const VectorType &vector() const

inline MapType &map()

inline const MapType &map() const

Private Members

VectorType vector_: Container for unstamped data.

MapType map_: Container for stamped data.

bool has_stamps_: Store if *this contains stamped or unstamped data.

bool has_poses_: Store if *this contains motions or poses.

Friends

friend class M3DReader

friend std::ostream &operator<<(std::ostream &os, const TransformContainer &container): Inserts a description of *this.

struct const_item_iterator

Public Types

using iterator_category = std::forward_iterator_tag

using difference_type = std::ptrdiff_t

using key_type = StampType 

using value_type = std::pair<const StampType, DataTypeConst>

using pointer = const value_type*

using reference = value_type 

Public Functions

explicit const_item_iterator(MapType::const_iterator map_it): Const Item Iterator.

reference operator*() const

pointer operator->() const

const_item_iterator &operator++()

const_item_iterator operator++(int)

Private Members

MapType::const_iterator map_it_

Friends

friend class TransformContainer

friend bool operator==(const const_item_iterator &a, const const_item_iterator &b)

friend bool operator!=(const const_item_iterator &a, const const_item_iterator &b)

struct const_iterator

Public Types

using iterator_category = std::forward_iterator_tag

using difference_type = std::ptrdiff_t

using value_type = DataTypeConst 

using pointer = DataTypeConst 

using reference = DataTypeConst 

Public Functions

explicit const_iterator(VectorType::const_iterator vector_it): Const Iterator.

explicit const_iterator(MapType::const_iterator map_it)

reference operator*() const

pointer operator->() const

const_iterator &operator++()

const_iterator operator++(int)

Private Members

VectorType::const_iterator vector_it_

MapType::const_iterator map_it_

bool is_map_

Friends

friend class TransformContainer

friend bool operator==(const const_iterator &a, const const_iterator &b)

friend bool operator!=(const const_iterator &a, const const_iterator &b)

struct iterator

Iterator Definitions.

Public Types

using iterator_category = std::forward_iterator_tag

using difference_type = std::ptrdiff_t

using value_type = DataType 

using pointer = DataType 

using reference = DataType&

Public Functions

explicit iterator(VectorType::iterator vector_it): Iterator.

explicit iterator(MapType::iterator map_it)

reference operator*() const

pointer operator->() const

iterator &operator++()

iterator operator++(int)

Private Members

VectorType::iterator vector_it_

MapType::iterator map_it_

bool is_map_

Friends

friend class TransformContainer

friend bool operator==(const iterator &a, const iterator &b)

friend bool operator!=(const iterator &a, const iterator &b)