ncore.geometry Package#

Package exposing NCore’s geometry utilities: pose interpolation and motion compensation, rigid transformations of points and bounding boxes, and time intervals

class ncore.geometry.HalfClosedInterval(start: int, stop: int)#

Bases: object

Represents a half closed interval [start, stop) of integers

cover_range(
sorted_samples: ndarray,
) → range#

Given a set of sorted samples (not validated), return the corresponding range for samples that are within the interval

property end: int#

Returns the inclusive end of the interval (i.e. stop - 1).

static from_start_end(
start: int,
end: int,
) → HalfClosedInterval#

Creates a half-closed interval from start and end (inclusive)

intersection(
other: HalfClosedInterval,
) → HalfClosedInterval | None#

Computes the intersection of two half-closed interval

overlaps(
other: HalfClosedInterval,
) → bool#

Checks if the interval has a non-zero overlap with an other closed interval

start: int#
stop: int#
class ncore.geometry.MotionCompensator(
pose_graph: PoseGraphInterpolator,
parameters: Parameters | None = None,
)#

Bases: object

Performs motion compensation / decompensation of points measured relative to time-dependent frames

class MotionCompensationResult(
xyz_s_reftime: 'np.ndarray',
xyz_e_reftime: 'np.ndarray',
reference_frame_id: 'str',
reference_timestamp_us: 'int',
anchor_frame_id: 'str',
)#

Bases: object

anchor_frame_id: str#
reference_frame_id: str#
reference_timestamp_us: int#
xyz_e_reftime: ndarray#
xyz_s_reftime: ndarray#
class Parameters(
method: Method = Method.EXACT,
interpolation_tolerance: float = 1e-05,
)#

Bases: object

Parameters of the motion compensation

class Method(value)#

Bases: IntEnum

How the per-point transformations are evaluated

EXACT = 1#

interpolates the poses at each distinct point timestamp

INTERPOLATED = 2#

interpolates the poses at the timestamps of a grid spanning the points’ timestamps, and linearly interpolates the transformations of the points in between (faster, see interpolation_tolerance)

interpolation_tolerance: float = 1e-05#

Maximum deviation of the points of the INTERPOLATED method from the exact transformations, in the units of the pose graph’s translations (e.g. meters), in addition to the float32 rounding of both methods. The grid is refined from an estimate of the deviation (see _InterpolationGrid.refined), which holds for motions that are smooth between the pose timestamps (as interpolated by the pose graph). Smaller tolerances refine the grid at practically no cost (the per-point work doesn’t depend on the grid size).

method: Method = 1#

How the per-point transformations are evaluated

static from_sensor_rig(
sensor_id: str,
T_sensor_rig: ndarray,
T_rig_worlds: ndarray,
T_rig_worlds_timestamps_us: ndarray,
anchor_frame_id: str = 'world',
rig_frame_id: str = 'rig',
parameters: Parameters | None = None,
) → MotionCompensator#

Constructs a motion-compensator from a sensor-rig transformations for a specific sensor only

Parameters:

  • sensor_id (str) – id of the sensor node

  • T_sensor_rig (np.ndarray) – static sensor -> rig transform, [4, 4]

  • T_rig_worlds (np.ndarray) – time-dependent rig -> anchor transforms, [N, 4, 4]

  • T_rig_worlds_timestamps_us (np.ndarray) – timestamps for the rig -> anchor transforms, [N]

  • anchor_frame_id (str) – name of the common anchor frame node (defaults to “world”). Pass the same value as anchor_frame_id to the (de)compensation methods.

  • rig_frame_id (str) – name of the rig frame node (defaults to “rig”).

  • parameters (Optional[MotionCompensator.Parameters]) – parameters of the motion compensation

motion_compensate_points(
sensor_id: str,
xyz_pointtime: np.ndarray,
timestamp_us: np.ndarray,
frame_start_timestamp_us: int,
frame_end_timestamp_us: int,
reference_timestamp_us: int | None = None,
anchor_frame_id: str = 'world',
) → MotionCompensationResult#

Perform motion compensation of points in time-dependent sensor frame at measurement time to a common reference sensor frame

By default the reference is the end of the frame (frame_end_timestamp_us). Pass reference_timestamp_us to compensate to a different reference frame (e.g. the start of the frame). The chosen reference must be inverted by motion_decompensate_points() using the same reference_timestamp_us and anchor_frame_id.

Parameters:

  • sensor_id (str) – sensor the points are relative to. This is also the reference frame the compensated points are expressed in (reference_frame_id).

  • xyz_pointtime (np.ndarray) – points in time-dependent sensor frame (~before motion compensation), [N,3]

  • timestamp_us (np.ndarray) – timestamps of points, [N]

  • frame_start_timestamp_us (int) – frame start timestamp

  • frame_end_timestamp_us (int) – frame end timestamp

  • reference_timestamp_us (Optional[int]) – timestamp of the reference sensor frame to compensate to; defaults to frame_end_timestamp_us. Must lie within [frame_start_timestamp_us, frame_end_timestamp_us].

  • anchor_frame_id (str) – common anchor frame used to compose the transforms (defaults to “world”). Use a non-default anchor when the pose graph is anchored at a frame other than “world”.

Returns:

result of the motion-compensation

Return type:

MotionCompensationResult

motion_decompensate_points(
sensor_id: str,
xyz_reftime: ndarray,
timestamp_us: ndarray,
frame_start_timestamp_us: int,
frame_end_timestamp_us: int,
reference_timestamp_us: int | None = None,
anchor_frame_id: str = 'world',
) → ndarray#

Decompensate motion of motion-compensated points to bring points into time-dependent sensor-frame

The input points are assumed to be expressed in the sensor frame at a single reference timestamp (the timestamp the data was motion-compensated to). By default this reference is the end of the frame (frame_end_timestamp_us), matching motion_compensate_points(). Datasets that compensate to a different reference (e.g. the start of the frame) can pass reference_timestamp_us explicitly. anchor_frame_id must match the value passed to motion_compensate_points().

Parameters:

  • sensor_id (str) – sensor the points are relative to

  • xyz_reftime (np.array) – motion-compensated points relative to the reference-time sensor frame to be decompensated, [N,3]

  • timestamp_us (np.ndarray) – timestamps of points, [N]

  • frame_start_timestamp_us (int) – frame start timestamp

  • frame_end_timestamp_us (int) – frame end timestamp

  • reference_timestamp_us (Optional[int]) – timestamp of the sensor frame the points are expressed in; defaults to frame_end_timestamp_us. Must lie within [frame_start_timestamp_us, frame_end_timestamp_us].

  • anchor_frame_id (str) – common anchor frame used to compose the transforms (defaults to “world”). Must match the anchor used for compensation.

Returns:

points in time-dependent sensor frame after motion-decompensation [n,3]

Return type:

xyz_pointtime(np.array)

property parameters: Parameters#

The parameters of the motion compensation

class ncore.geometry.PoseGraphInterpolator(edges: List[Edge])#

Bases: object

Interpolates rigid poses between named nodes in a pose graph. Edges in the pose graph can be static or dynamic (time-dependent)

class Edge(
source_node: str,
target_node: str,
T_source_target: npt.NDArray[np.floating],
timestamps_us: npt.NDArray[np.uint64] | None,
)#

Bases: object

Represents an edge in the pose graph, either static or dynamic (time-dependent).

The computational data-type is given by the input arrays.

get_T(
source_node: str,
target_node: str,
timestamps_us: npt.NDArray[np.uint64],
) → npt.NDArray[np.floating]#

Returns the transformation from source to target node at the given timestamps

Parameters:

  • source_node (str) – name of the source node

  • target_node (str) – name of the target node

  • timestamps_us (np.ndarray) – timestamps at which to evaluate the pose - defines the batch shape

Returns:

evaluated poses from source to target node at the given timestamps [batch-shape,4,4]

in the data-type of the edge

Return type:

np.ndarray

interpolator: PoseInterpolator | None#
evaluate_poses(
source_node: str,
target_node: str,
timestamps_us: npt.NDArray[np.uint64],
) → npt.NDArray[np.floating]#

Evaluates relative pose from source to target node frames at the given timestamps. Performs graph traversal and pose evaluation / composition (interpolation for time-dependent edges) as needed.

Parameters:

  • source_node (str) – name of the source node

  • target_node (str) – name of the target node

  • timestamps_us (np.ndarray) – timestamps at which to evaluate the pose - defines the batch shape

Returns:

evaluated poses from source to target node at the given timestamps [batch-shape,4,4]

Return type:

np.ndarray

get_edge(
source_node: str,
target_node: str,
normalized: bool = False,
) → Edge | None#

Returns the edge between the given source and target node, or None if no such edge exists

Parameters:

  • source_node (str) – name of the source

  • target_node (str) – name of the target

  • normalized (bool) – if True, looks up the edge in normalized (sorted) manner

Returns:

edge between source and target node, or None if no such edge exists

Return type:

Optional[Edge]

get_path_timestamps_us(
source_node: str,
target_node: str,
) → List[npt.NDArray[np.uint64]]#

Returns the pose timestamps of the time-dependent edges on the path between two nodes (the timestamps at which the interpolated relative pose of the nodes may change its motion)

property nodes: Set[str]#

Returns the set of nodes in the pose graph

static normalize_node_pair(
node_a: str,
node_b: str,
) → Tuple[str, str]#

Returns a normalized (sorted) tuple of the given node names

Parameters:

  • node_a (str) – name of the first node

  • node_b (str) – name of the second node

Returns:

normalized (sorted) tuple of the given node names

Return type:

Tuple[str, str]

property normalized_edge_map: Dict[Tuple[str, str], Edge]#

Returns the normalized (sorted by node names) edge map of the pose graph

class ncore.geometry.PoseInterpolator(poses, timestamps)#

Bases: object

Interpolates the poses to the desired time stamps. The translation component is interpolated linearly, while spherical linear interpolation (SLERP) is used for the rotations. https://en.wikipedia.org/wiki/Slerp

extrapolate_to_timestamps(
ts_target: np.ndarray,
max_extrapolation_time_us: int = 1000000,
dtype: npt.DTypeLike = <class 'numpy.float32'>,
) → np.ndarray#

Extrapolates/interpolates poses to target timestamps, allowing extrapolation beyond the original range.

This requires the original timestamps to be in absolute microseconds (us / int type).

For timestamps within the original range, standard interpolation is used. For timestamps outside the range, linear extrapolation is used for translation and SLERP-based extrapolation (constant angular velocity) is used for rotation.

Parameters:

  • ts_target – Target timestamps for which poses will be computed [m; us]

  • max_extrapolation_time_us – Maximum allowed extrapolation time in microseconds (default: 1 second). Raises ValueError if any timestamp exceeds this limit.

  • dtype – Data type for the output poses and the extrapolation computations

Returns:

Extrapolated/interpolated poses in SE3 representation [m, 4, 4]

Return type:

np.ndarray

Raises:

ValueError – If any timestamp requires extrapolation beyond max_extrapolation_time_us

in_range(ts) → bool#

Returns true if all provided timestamps (scalar or array-like) are within the interpolation range

interpolate_to_timestamps(
ts_target,
dtype: npt.DTypeLike = <class 'numpy.float32'>,
) → np.ndarray#

Interpolates poses to target timestamps. :param ts_target: Target timestamps for which poses will be computed in the same time-domain as the original timestamps [m] :param dtype: Data type for the output poses

Returns:

Interpolated poses in SE3 representation [m, 4, 4]

Return type:

np.ndarray

Raises:

ValueError – If any timestamp is outside the interpolation range

property poses: ndarray#

Returns the original poses used for interpolation

property timestamps: ndarray#

Returns the timestamps corresponding to the original poses used for interpolation

ncore.geometry.bbox_pose(bbox: ndarray) → ndarray#

Converts an array-encoded bounding-box into a corresponding pose

ncore.geometry.is_within_3d_bboxes(
pc: ndarray,
bboxes: ndarray,
) → ndarray#

Checks whether points of a point cloud are in an array of 3d boxes

Parameters:

  • pc – [N, 3] tensor. Inner dims are: [x, y, z].

  • bboxes – [M, 9] tensor. Inner dims are: [center_x, center_y, center_z, length, width, height, roll, pitch, yaw]. roll/pitch/yaw are in radians.

Returns:

point_in_bboxes; [N,M] boolean array.

ncore.geometry.pose_bbox(
pose: ndarray,
dimensions: ndarray,
) → ndarray#

Converts a pose with extents to an array-encoded bounding-box

ncore.geometry.se3_inverse(T: ndarray, unbatch: bool = True) → ndarray#

Computes the inverse of multiple rigid transformations

Parameters:

  • Ts (np.ndarray) – se3 transformation matrices to invert [N, 4, 4] or [4, 4]

  • unbatch (bool) – if the single matrix should be unbatched (first dimension removed) or not

Returns:

Inverse transformations [N, 4, 4] or [4, 4]

Return type:

(np array)

ncore.geometry.so3_trans_2_se3(so3, trans) → ndarray#

Create a 4x4 rigid transformation matrix given so3 rotation and translation.

Parameters:

  • so3 – rotation matrix [n,3,3]

  • trans – x, y, z translation [n, 3]

Returns:

the constructed transformation matrix [n,4,4]

Return type:

np.ndarray

ncore.geometry.time_bounds(
timestamps_us: List[int],
seek_sec: float | None,
duration_sec: float | None,
) → tuple[int, int]#

Determine start and end timestamps given optional seek and duration times

Parameters:

  • timestamps_us – list of all available timestamps (in microseconds)

  • seek_sec – Optional: if non-None, the time (in seconds) to skip starting from the first timestamp

  • duration_sec – Optional: if non-None, the total time (in seconds) between the start and end time bounds

Returns:

first valid timestamp in restricted bounds (in microseconds) end_timestamp_us: last valid timestamp in restricted bounds (in microseconds)

Return type:

start_timestamp_us

ncore.geometry.transform_bbox(
bbox_source: ndarray,
T_source_target: ndarray,
) → ndarray#

Applies a rigid-transformation to a bounding box :param bbox: bounding-box in source-frame parameterized by [x, y, z, length, width, height, eulerX, eulerY, eulerZ] :type bbox: np.ndarray :param T: se3 source->target transformation matrix to apply [4,4] :type T: np.array

Out:

(np array): transformed bounding-box [num_pts, 3] or [bs, num_pts, 3]

ncore.geometry.transform_point_cloud(pc: ndarray, T) → ndarray#
ncore.geometry.transform_point_cloud(pc: torch.Tensor, T) → torch.Tensor
Transform the point cloud with the provided transformation matrix,

support torch.Tensor and np.ndarry.

The returned type matches the type of pc: a numpy array in, a numpy array out; a torch tensor in, a torch tensor out.

Parameters:

  • pc (np.array) – point cloud coordinates (x,y,z) [num_pts, 3] or [bs, num_pts, 3]

  • T (np.array) – se3 transformation matrix [4, 4] or [bs, 4, 4]

Out:

(np array): transformed point cloud coordinated [num_pts, 3] or [bs, num_pts, 3]