Class PoseTreeEdgeHistory

• Defined in File pose_tree_history.hpp

Nested Types

• Struct PoseTreeEdgeHistory::TimedPose

class PoseTreeEdgeHistory

Class that stores a history of poses using pre-allocated memory and a cyclic buffer.

Provides helper functions to add a new pose or query the pose at a given time. The history uses a circular buffer to efficiently manage memory and provides various interpolation methods for accessing poses at specific times.

Public Types

enum class AccessMethod

Interpolation method for accessing poses in the pose tree.

Values:

enumerator kNearest

Gets the value of the closest sample.

enumerator kInterpolateLinearly

Interpolates linearly between adjacent samples. If the query is outside the validity range, the closest pose will be returned.

enumerator kExtrapolateLinearly

Inter- or extrapolates linearly based on neighbouring samples. This require at least two valid poses or Error::kOutOfRange will be returned.

enumerator kInterpolateSlerp

Interpolates with slerp between adjacent samples. If the query is outside the validity range, the closest pose will be returned.

enumerator kExtrapolateSlerp

Inter- or extrapolates with slerp based on neighbouring samples. This require at least two valid poses or Error::kOutOfRange will be returned.

enumerator kPrevious

Use the latest Pose before a given time.

enumerator kDefault

Fallback to the default interpolation.

enum class Error

Error codes used by this class.

Values:

enumerator kInvalidArgument

kInvalidArgument is returned when a function is called with argument that does not make sense such as querying a pose outside the valid range or provide a wrong interpolation method.

enumerator kOutOfOrder

kOutOfOrder is returned when a set/disconnected called is made with a version or time lower than the latest TimedPose. Both time and version must be strictly increasing.

enumerator kFramesNotLinked

kFramesNotLinked is returns if a get query is made and the tree is not connected at the given time and version of the edge.

enumerator kOutOfRange

kOutOfRange is returned if not enough poses are available to do extrapolation or if the available buffer is too small to store a new pose.

template<typename T>
using expected_t = expected<T, Error>

Expected type used by this class.

using unexpected_t = unexpected<Error>

Unexpected type used by this class.

using frame_t = uint64_t

Type used to uniquely identify a frame.

using version_t = uint64_t

Type used for versioning the edge.

Public Functions

PoseTreeEdgeHistory() = default

Default constructor used to be able to pre-allocate memory.

PoseTreeEdgeHistory(frame_t lhs, frame_t rhs, int32_t maximum_size, TimedPose *buffer)

Constructor to create a usable PoseTreeEdgeHistory object.

Parameters

• lhs – Left hand side frame identifier.

• rhs – Right hand side frame identifier.

• maximum_size – Maximum number of poses this edge can store.

• buffer – Pre-allocated buffer that can hold maximum_size elements.

PoseTreeEdgeHistory(frame_t lhs, frame_t rhs, int32_t maximum_size, AccessMethod access_method, TimedPose *buffer)

Constructor to create a usable PoseTreeEdgeHistory object with custom access method.

Parameters

• lhs – Left hand side frame identifier.

• rhs – Right hand side frame identifier.

• maximum_size – Maximum number of poses this edge can store.

• access_method – Default access method for interpolation.

• buffer – Pre-allocated buffer that can hold maximum_size elements.

expected_t<void> set(double time, const Pose3d &pose, version_t version)

Set the pose at a given time.

If the array is empty Error::kOutOfMemory will be returned, otherwise if a pose already exists and time or version <= pose.time/version then Error::kOutOfOrder is returned. Otherwise it will succeed, and if the history already contained maximum_size_ elements, then the oldest pose will be forgotten.

Parameters

• time – Time at which to set the pose.

• pose – 3D pose transformation.

• version – Version ID for this pose update.

Returns

Success or error status.

expected_t<TimedPose> at(int32_t index) const

Get the TimedPose at a given position in the history.

Parameters

index – Index of the pose to retrieve.

Returns

TimedPose on success, Error::kInvalidArgument if index is negative, Error::kOutOfRange if index >= size.

expected_t<Pose3d> get(double time, AccessMethod method, version_t version) const

Get the Pose3d at a given time using the given version of the PoseTree.

If no pose existed at the given time, Error::kFramesNotLinked will be returned. The desired method can be provided, for kExtrapolateLinearly, at least two poses are required.

Parameters

• time – Time at which to query the pose.

• method – Interpolation method to use.

• version – Version of the PoseTree to query.

Returns

Pose3d on success, error on failure.

expected_t<void> disconnect(double time, version_t version)

Disconnect a frame at a given time.

Parameters

• time – Time at which to disconnect the frames.

• version – Version ID for this disconnection.

Returns

Success or error status.

bool connected() const

Check if the frames are currently connected.

Returns

True if frames are connected, false otherwise.

void reset()

Reset the history, erasing all poses.

expected_t<TimedPose> latest() const

Get information about the latest pose.

Returns

Latest TimedPose on success, error if no poses exist.

inline const TimedPose *data() const

Get a pointer to the internal buffer.

Returns

Const pointer to the TimedPose buffer.

inline int32_t size() const

Get the current number of poses stored.

Returns

Current size of the history.

inline int32_t maximum_size() const

Get the maximum number of poses this edge can contain.

Returns

Maximum size of the history buffer.

inline frame_t lhs() const

Get the left hand side frame identifier.

This edge represents the transformation from the rhs frame to the lhs frame.

Returns

UID of the lhs frame.

inline frame_t rhs() const

Get the right hand side frame identifier.

This edge represents the transformation from the rhs frame to the lhs frame.

Returns

UID of the rhs frame.

struct TimedPose

Helper structure to store the pose at a given time on the edge.

Public Members

Pose3d pose

3D pose that transforms the lhs frame into the rhs frame.

double time

Time of the pose. Needs to be strictly increasing.

version_t version

Version ID of the pose. Needs to be strictly increasing.

bool valid

If false, then it marks the edge as being disconnected from this current time. The pose does not matter.