holoscan::PoseTreeEdgeHistory

Beta

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.

#include <holoscan/pose_tree_history.hpp>

Constructors

PoseTreeEdgeHistory

Default

Overload 2

Overload 3

holoscan::PoseTreeEdgeHistory::PoseTreeEdgeHistory() = default

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

Methods

set

expected_t<void> holoscan::PoseTreeEdgeHistory::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.

Returns: Success or error status.

Parameters

time

double

Time at which to set the pose.

pose

const Pose3d &

3D pose transformation.

version

version_t

Version ID for this pose update.

at

expected_t<TimedPose> holoscan::PoseTreeEdgeHistory::at(
    int32_t index
) const

Get the TimedPose at a given position in the history.

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

Parameters

index

int32_t

Index of the pose to retrieve.

get

expected_t<Pose3d> holoscan::PoseTreeEdgeHistory::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.

Returns: Pose3d on success, error on failure.

Parameters

time

double

Time at which to query the pose.

method

AccessMethod

Interpolation method to use.

version

version_t

Version of the PoseTree to query.

disconnect

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

Disconnect a frame at a given time.

Returns: Success or error status.

Parameters

time

double

Time at which to disconnect the frames.

version

version_t

Version ID for this disconnection.

connected

bool holoscan::PoseTreeEdgeHistory::connected() const

Check if the frames are currently connected.

Returns: True if frames are connected, false otherwise.

reset

void holoscan::PoseTreeEdgeHistory::reset()

Reset the history, erasing all poses.

latest

expected_t<TimedPose> holoscan::PoseTreeEdgeHistory::latest() const

Get information about the latest pose.

Returns: Latest TimedPose on success, error if no poses exist.

data

const TimedPose * holoscan::PoseTreeEdgeHistory::data() const

Get a pointer to the internal buffer.

Returns: Const pointer to the TimedPose buffer.

size

int32_t holoscan::PoseTreeEdgeHistory::size() const

Get the current number of poses stored.

Returns: Current size of the history.

maximum_size

int32_t holoscan::PoseTreeEdgeHistory::maximum_size() const

Get the maximum number of poses this edge can contain.

Returns: Maximum size of the history buffer.

lhs

frame_t holoscan::PoseTreeEdgeHistory::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.

rhs

frame_t holoscan::PoseTreeEdgeHistory::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.

reserve_new_pose

expected_t<int32_t> holoscan::PoseTreeEdgeHistory::reserve_new_pose(
    double time,
    version_t version
)

Reserve a new pose for a given time/version and return its index.

If there exists another pose with a later time/version, it returns kOutOfOrder.

Returns: Index of the reserved pose on success, error on failure.

Parameters

time

double

Time for the new pose.

version

version_t

Version for the new pose.

Types

Typedefs

Name	Definition	Description
`expected_t`	`expected< T, Error >`	Expected type used by this class.
`unexpected_t`	`unexpected< Error >`	Unexpected type used by this class.
`frame_t`	`uint64_t`	Type used to uniquely identify a frame.
`version_t`	`uint64_t`	Type used for versioning the edge.

AccessMethod

Interpolation method for accessing poses in the pose tree.

Name	Value	Description
`kNearest`		Gets the value of the closest sample.
`kInterpolateLinearly`		Interpolates linearly between adjacent samples.
`kExtrapolateLinearly`		Inter- or extrapolates linearly based on neighbouring samples.
`kInterpolateSlerp`		Interpolates with slerp between adjacent samples.
`kExtrapolateSlerp`		Inter- or extrapolates with slerp based on neighbouring samples.
`kPrevious`		Use the latest Pose before a given time.
`kDefault`		Fallback to the default interpolation.

Error

Error codes used by this class.

Name	Value	Description
`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.
`kOutOfOrder`		kOutOfOrder is returned when a set/disconnected called is made with a version or time lower than the latest TimedPose.
`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.
`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.

Member variables

Name	Type	Description
`mutex_`	`std::shared_timed_mutex`	Mutex to protect changes to this object.
`edges_info_`	`TimedPose *`	Pointer to the buffer that contains the list of TimedPose (we use a circular buffer to access it).
`lhs_`	`frame_t`	Name of the frame this edge connects to.
`rhs_`	`frame_t`
`maximum_size_`	`int32_t`	Size of the buffer, aka maximum number of elements this edge can store at the same time.
`size_`	`int32_t`	Current number of poses on this edge.
`pos_`	`int32_t`	Position of the first pose in the circular buffer.
`default_access_method_`	`AccessMethod`	Default access method.

Inner classes

TimedPose

struct holoscan::PoseTreeEdgeHistory::TimedPose

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

Name	Type	Description
`pose`	`Pose3d`	3D pose that transforms the lhs frame into the rhs frame.
`time`	`double`	Time of the pose. Needs to be strictly increasing.
`version`	`version_t`	Version ID of the pose. Needs to be strictly increasing.
`valid`	`bool`	If false, then it marks the edge as being disconnected from this current time.