ouster.sdk.util.pose_util module¶

no_scipy()[source]¶

Checks the scipy availability with a warning message. :rtype: bool

Return type:: bool

normalize_vector(v)[source]¶

Return type:: ndarray
Parameters:: v (ndarray)

exp_rot_vec(vec)[source]¶

Converts so3 vector to a rotation matrix.

Parameters:: vec (ndarray) – so3 rotation vector [3] or vectors [N, 3] to rotation matrix [3, 3] or matrices [N, 3, 3]
Return type:: ndarray
Returns:: rotation matrix or matrices

log_rot_mat(rm)[source]¶

Convert rotation matrix to so3 coordinates (i.e. log() operator)

Parameters:: rm (ndarray) – rotation matrix [3, 3] or matrices [N, 3, 3]
Return type:: ndarray
Returns:: so3 coordinate rotation vector [3] or [N, 3]

exp_pose6(pose6)[source]¶

Convert exponential poses to homogeneous matrix poses.

Parameters:: pose6 (ndarray) – vector [6] or matrix [N, 6] of exponential poses
Return type:: ndarray
Returns:: Homogeneous matrix poses of size [4, 4] or [N, 4, 4].

log_pose(pose)[source]¶

Convert homogeneous matrix(s) to exp pose coordinates.

Parameters:: pose (ndarray) – homogeneous pose [4, 4] or poses [N, 4, 4]
Return type:: ndarray
Returns:: exp pose coordinates [6] or [N, 6]

pose_interp(p1, p2, t, *, delta_pose6=None)[source]¶

Pose interpolation between pose1 and pose2 at time t as ratio.

Parameters:

p1 (ndarray) – starting pose
p2 (ndarray) – ending pose
t (float) – ratio between pose p1 and p2 at what point to interpolate, not restricted between [0, 1] and can be extended for out of bounds
delta_pose6 (Optional[ndarray]) – pre-calculated difference inv(p1) @ p2, saves computation if it’s available already

Return type:

ndarray

Returns:

pose of the point at time t on the line defined by p1 and p2 on SE3 manifold

traj_interp(traj_poses, ts)[source]¶

Trajectory interpolation for points in between.

TODO[pb]: Extend with time_bounds args for traj evaluator when needed :rtype: ndarray

Parameters:

traj_poses (Sequence[Tuple[int | float | number, ndarray]])
ts (Sequence[int | float | number] | ndarray)

Return type:

ndarray

class Poser(*args, **kwargs)[source]¶

Bases: Protocol

Actor that adds poses to LidarFrames

class TrajectoryEvaluator(poses, *, time_bounds=0)[source]¶

Bases: Poser

Interpolates trajectory for a set of timestamps from knot poses.

TODO[pb]: Add function to add/remove knot poses from traj eval.

TODO: Optionally, we may want to implement these calculations in C++ and: use bindings to make it faster.

Parameters:

poses (Sequence[Tuple[Union[int, float, number], ndarray]]) – List of knot poses with timestamps. Every list item is a tuple (ts, pose).
time_bounds (Optional[float]) –
whether to restrict the pose interpolation to the timestamp range within the poses list: None - no restriction at all on the timestamps that can

be used to get pose from the trajectory

0 - strict bounds on the timestamp range in the
poses list

>0 - ratio that is allowed to go over the timestamp
bounds. ratio value is applied as the ratio of pose[1].ts - pose[0].ts for the left bound, and pose[N].ts - pose[N-1].ts for the right bound.

pose_at(ts)[source]¶

Calculates a single pose (4x4 matrix) at a given ts timestamp. :rtype: ndarray

Parameters:: ts (int | float | number)
Return type:: ndarray

poses_at(ts)[source]¶

Calculates multiple poses (4x4 matrices) at a given ts timestamps. :rtype: ndarray

Parameters:: ts (Sequence[int | float | number] | ndarray)
Return type:: ndarray

pose_frames(source, *, poses=None)[source]¶

Add poses to LidarFrames stream.

Parameters:

source – one of: - Sequence[core.LidarFrame] - single frame sources - Sequence[core.FrameSet] - multi frame set sources
poses (Poser | None)

load_kitti_poses(file)[source]¶

Loads the Kitti poses from the file.

Return type:: ndarray
Returns:: [N, 4, 4] array of homogeneous poses
Parameters:: file (str)

make_kiss_traj_poses(poses)[source]¶

Makes a traj poses from kiss poses.

Parameters:: poses (Union[Sequence[ndarray], ndarray]) – pose for every frame in the sequence as returned by KissICP
Returns:: 0.5 For example frame indexes 0, 1, 2 produce timestamps 0.5, 1.5, 2.5
Return type:: trajectory poses timestamped by the frame index mid point

pose_frames_from_kitti(source, kitti_poses)[source]¶

Add poses to LidarFrames stream using the previously saved per frame poses.

Every pose is considered to be in the middle of the frame. We assume that very first frame starts at t = 0 and ends at t = 1, thus the first pose is timestamped as 0.5, second pose is timestamped at 1.5 (middle of the second frame), and so on … to the very last pose N which timestamped at N + 0.5 for the last N frame.

Parameters:

source – one of: - Sequence[core.LidarFrame] - single frame sources - Sequence[core.FrameSet] - multi frame set sources
kitti_poses (str) – path to the file with in kitti poses format, i.e. every line contains 12 floats of 4x4 homogeneous transformation matrix ([:3, :] in numpy notation, row-major serialized)