ouster.sdk.util.pose_util module

no_scipy()

Checks the scipy availability with a warning message.

Return type:

bool

normalize_vector(v)

Return type:

ndarray

exp_rot_vec(vec)

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)

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)

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)

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)

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)

Trajectory interpolation for points in between.

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

Return type:

ndarray

class Poser(*args, **kwargs)

Bases: Protocol

Actor that adds poses to LidarScans

class TrajectoryEvaluator(poses, *, time_bounds=0)

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)

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

Return type:

ndarray

poses_at(ts)

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

Return type:

ndarray

get_rot_matrix_to_align_to_gravity(accel_x, accel_y, accel_z)

Computes the rotation matrix needed to align a given acceleration vector with the direction of gravity, fixing the yaw angle to zero.

Parameters:

• accel_x (float) – x-component of the acceleration vector.

• accel_y (float) – y-component of the acceleration vector.

• accel_z (float) – z-component of the acceleration vector.

Returns:

A 3x3 rotation matrix that aligns the acceleration vector with the gravity vector [0,0,1] while fixing the yaw angle to zero.

pose_scans(source, *, poses=None)

Add poses to LidarScans stream.

Parameters:

source – one of: - Sequence[core.LidarScan] - single scan sources - Sequence[core.LidarScanSet] - multi scans sources

load_kitti_poses(file)

Loads the Kitti poses from the file.

Return type:

ndarray

Returns:

[N, 4, 4] array of homogeneous poses

make_kiss_traj_poses(poses)

Makes a traj poses from kiss poses.

Parameters:

poses (Union[Sequence[ndarray], ndarray]) – pose for every scan in the sequence as returned by KissICP

Returns:

0.5 For example scan indexes 0, 1, 2 produce timestamps 0.5, 1.5, 2.5

Return type:

trajectory poses timestamped by the scan index mid point

pose_scans_from_kitti(source, kitti_poses)

Add poses to LidarScans stream using the previously saved per scan poses.

Every pose is considered to be in the middle of the scan. We assume that very first scan 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 scan), and so on … to the very last pose N which timestamped at N + 0.5 for the last N scan.

Parameters:

• source – one of: - Sequence[core.LidarScan] - single scan sources - Sequence[core.LidarScanSet] - multi scans 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)