apairo-preprocess 0.2.0__tar.gz → 0.4.0__tar.gz

{apairo_preprocess-0.2.0 → apairo_preprocess-0.4.0}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: apairo-preprocess
-Version: 0.2.0
+Version: 0.4.0
 Summary: Preprocessing for apairo datasets: odometry, ground segmentation, projection, traversability labels
 Author-email: Augustin Bresset <augustin.bresset@gmail.com>
 License: MIT
@@ -58,13 +58,40 @@ Requires Python ≥ 3.11.
 
 ## Preprocessors
 
-### Odometry
+### Poses
+
+Labels computed from a trajectory are only as good as the trajectory. When a
+dataset ships its own poses (an INS, a GNSS-INS, the trajectory of its
+release), use them: turning them into one pose per scan is interpolation and
+composition, with fixed rules and no parameter to tune per sensor.
+
+| Function / class | Output | Source |
+|---|---|---|
+| `reference_poses` / `ReferencePoses` | `(N, 4, 4)` float64 poses + `(N,)` bool validity, on the scan clock | pose rows delivered with the dataset |
+| `pose_validity` | `(N,)` bool | physical bounds: top speed, turn rate |
+
+A scan gets a pose only when two rows bracket its time, at most `max_gap`
+apart; nothing is extrapolated; a step faster than `max_speed` invalidates
+both of its scans; an invalid scan's pose is NaN. The rows and the scans are on
+two clocks, so the function is called directly and its outputs written with
+`apairo.ChannelWriter` (see the quickstart).
+
+Lidar odometry estimates a trajectory instead. It returns a pose even where it
+has lost track, with no signal of it, and its drift depends on the terrain:
 
 | Class | Output channel | Backend | Output |
 |---|---|---|---|
 | `KissICPOdometry` | `kissicp_poses` | [KISS-ICP](https://github.com/PRBonn/kiss-icp) | `(4, 4)` float64 pose per scan |
 | `GICPOdometry` | `gicp_poses` | Open3D GICP | `(4, 4)` float64 pose per scan |
 
+Measured on two TartanDrive 2.0 recordings against the vehicle's INS and the
+release's own SLAM trajectory: the future path seen from a scan agrees within
+5-10 cm (median) at 10 m ahead for every source, but sources drift apart with
+the look-ahead, to 1.2-1.5 m (p95) at 40 m. In vegetation, KISS-ICP was the one
+that drifted. Use lidar odometry for recordings without delivered poses, keep
+`forward_distance` short (15 m), and check it against another source when one
+exists.
+
 ### Ground segmentation
 
 Binary ground/non-ground labels (0 = ground, 1 = non-ground). All three algorithms share the same label convention and can be compared directly.
@@ -179,9 +206,41 @@ SemanticKittiDataset.run_preprocess(
 )
 ```
 
+### Poses delivered with a dataset
+
+```python
+import numpy as np
+from apairo import ChannelWriter
+from apairo_preprocess import ReferencePoses
+
+rows = np.load(f"{seq}/gps_odom/odometry.npy")        # [x y z qx qy qz qw, twist]
+row_times = np.loadtxt(f"{seq}/gps_odom/timestamps.txt")
+scan_times = np.loadtxt(f"{seq}/velodyne_0/timestamps.txt")
+
+ins = ReferencePoses(clock_offset=-0.05, max_gap=0.1, max_speed=20.0)
+poses, valid = ins(rows, row_times, scan_times)        # relative to the first scan
+
+with ChannelWriter(seq, "ins_poses", timestamps_from="velodyne_0", sources=["gps_odom"]) as w:
+    for i, t in enumerate(scan_times):
+        w.add(poses[i], stem=f"{i:06d}", timestamp=t)
+with ChannelWriter(seq, "ins_valid", timestamps_from="velodyne_0", sources=["gps_odom"]) as w:
+    for i, t in enumerate(scan_times):
+        w.add(np.asarray(valid[i]), stem=f"{i:06d}", timestamp=t)
+```
+
+`clock_offset` is the pose clock minus the lidar's, measured once per rig;
+`max_speed` is the robot's top speed. Pass the validity to the labeller as
+`valid_key`: an invalid scan then cuts the look-ahead, and with
+`unknown_label=255` its points read "unknown" rather than "not traversable".
+
 ### Traversability ground truth from trajectory
 
-Requires poses to be computed first (e.g. with `KissICPOdometry`).
+Requires poses first: delivered ones (above), or lidar odometry
+(`KissICPOdometry`) for a recording without any.
+
+Lidar odometry gives the pose of the *sensor*, not of the ground under the
+robot: pass the lidar's mounting height as `pose_height`, or every ground point
+lies below the height window and nothing is labelled traversable.
 
 ```python
 from apairo.dataset.goose import Goose3DDataset
@@ -189,12 +248,24 @@ from apairo_preprocess import KissICPOdometry, TraversabilityFromTrajectory
 
 Goose3DDataset.run_preprocess(KissICPOdometry(voxel_size=1.0), "/data/goose/seq_001")
 Goose3DDataset.run_preprocess(
-    TraversabilityFromTrajectory(poses_key="kissicp_poses", robot_radius=0.75),
+    TraversabilityFromTrajectory(
+        poses_key="kissicp_poses", robot_radius=0.75, pose_height=1.2
+    ),
     "/data/goose/seq_001",
 )
 # writes trav_traj/  (uint8: 1=traversable, 0=not)
 ```
 
+With delivered poses and their validity:
+
+```python
+TraversabilityFromTrajectory(
+    poses_key="ins_poses", valid_key="ins_valid", unknown_label=255,
+    robot_radius=0.8, pose_height=1.77, forward_distance=15.0,
+)
+# uint8: 1=traversable, 0=not, 255=unknown (scan without a trusted pose)
+```
+
 ### Traversability mask in image space
 
 Projects the trajectory ground truth into the camera: `trav_traj` labels the

{apairo_preprocess-0.2.0 → apairo_preprocess-0.4.0}/README.md

@@ -26,13 +26,40 @@ Requires Python ≥ 3.11.
 
 ## Preprocessors
 
-### Odometry
+### Poses
+
+Labels computed from a trajectory are only as good as the trajectory. When a
+dataset ships its own poses (an INS, a GNSS-INS, the trajectory of its
+release), use them: turning them into one pose per scan is interpolation and
+composition, with fixed rules and no parameter to tune per sensor.
+
+| Function / class | Output | Source |
+|---|---|---|
+| `reference_poses` / `ReferencePoses` | `(N, 4, 4)` float64 poses + `(N,)` bool validity, on the scan clock | pose rows delivered with the dataset |
+| `pose_validity` | `(N,)` bool | physical bounds: top speed, turn rate |
+
+A scan gets a pose only when two rows bracket its time, at most `max_gap`
+apart; nothing is extrapolated; a step faster than `max_speed` invalidates
+both of its scans; an invalid scan's pose is NaN. The rows and the scans are on
+two clocks, so the function is called directly and its outputs written with
+`apairo.ChannelWriter` (see the quickstart).
+
+Lidar odometry estimates a trajectory instead. It returns a pose even where it
+has lost track, with no signal of it, and its drift depends on the terrain:
 
 | Class | Output channel | Backend | Output |
 |---|---|---|---|
 | `KissICPOdometry` | `kissicp_poses` | [KISS-ICP](https://github.com/PRBonn/kiss-icp) | `(4, 4)` float64 pose per scan |
 | `GICPOdometry` | `gicp_poses` | Open3D GICP | `(4, 4)` float64 pose per scan |
 
+Measured on two TartanDrive 2.0 recordings against the vehicle's INS and the
+release's own SLAM trajectory: the future path seen from a scan agrees within
+5-10 cm (median) at 10 m ahead for every source, but sources drift apart with
+the look-ahead, to 1.2-1.5 m (p95) at 40 m. In vegetation, KISS-ICP was the one
+that drifted. Use lidar odometry for recordings without delivered poses, keep
+`forward_distance` short (15 m), and check it against another source when one
+exists.
+
 ### Ground segmentation
 
 Binary ground/non-ground labels (0 = ground, 1 = non-ground). All three algorithms share the same label convention and can be compared directly.
@@ -147,9 +174,41 @@ SemanticKittiDataset.run_preprocess(
 )
 ```
 
+### Poses delivered with a dataset
+
+```python
+import numpy as np
+from apairo import ChannelWriter
+from apairo_preprocess import ReferencePoses
+
+rows = np.load(f"{seq}/gps_odom/odometry.npy")        # [x y z qx qy qz qw, twist]
+row_times = np.loadtxt(f"{seq}/gps_odom/timestamps.txt")
+scan_times = np.loadtxt(f"{seq}/velodyne_0/timestamps.txt")
+
+ins = ReferencePoses(clock_offset=-0.05, max_gap=0.1, max_speed=20.0)
+poses, valid = ins(rows, row_times, scan_times)        # relative to the first scan
+
+with ChannelWriter(seq, "ins_poses", timestamps_from="velodyne_0", sources=["gps_odom"]) as w:
+    for i, t in enumerate(scan_times):
+        w.add(poses[i], stem=f"{i:06d}", timestamp=t)
+with ChannelWriter(seq, "ins_valid", timestamps_from="velodyne_0", sources=["gps_odom"]) as w:
+    for i, t in enumerate(scan_times):
+        w.add(np.asarray(valid[i]), stem=f"{i:06d}", timestamp=t)
+```
+
+`clock_offset` is the pose clock minus the lidar's, measured once per rig;
+`max_speed` is the robot's top speed. Pass the validity to the labeller as
+`valid_key`: an invalid scan then cuts the look-ahead, and with
+`unknown_label=255` its points read "unknown" rather than "not traversable".
+
 ### Traversability ground truth from trajectory
 
-Requires poses to be computed first (e.g. with `KissICPOdometry`).
+Requires poses first: delivered ones (above), or lidar odometry
+(`KissICPOdometry`) for a recording without any.
+
+Lidar odometry gives the pose of the *sensor*, not of the ground under the
+robot: pass the lidar's mounting height as `pose_height`, or every ground point
+lies below the height window and nothing is labelled traversable.
 
 ```python
 from apairo.dataset.goose import Goose3DDataset
@@ -157,12 +216,24 @@ from apairo_preprocess import KissICPOdometry, TraversabilityFromTrajectory
 
 Goose3DDataset.run_preprocess(KissICPOdometry(voxel_size=1.0), "/data/goose/seq_001")
 Goose3DDataset.run_preprocess(
-    TraversabilityFromTrajectory(poses_key="kissicp_poses", robot_radius=0.75),
+    TraversabilityFromTrajectory(
+        poses_key="kissicp_poses", robot_radius=0.75, pose_height=1.2
+    ),
     "/data/goose/seq_001",
 )
 # writes trav_traj/  (uint8: 1=traversable, 0=not)
 ```
 
+With delivered poses and their validity:
+
+```python
+TraversabilityFromTrajectory(
+    poses_key="ins_poses", valid_key="ins_valid", unknown_label=255,
+    robot_radius=0.8, pose_height=1.77, forward_distance=15.0,
+)
+# uint8: 1=traversable, 0=not, 255=unknown (scan without a trusted pose)
+```
+
 ### Traversability mask in image space
 
 Projects the trajectory ground truth into the camera: `trav_traj` labels the

{apairo_preprocess-0.2.0 → apairo_preprocess-0.4.0}/apairo_preprocess/__init__.py

@@ -1,7 +1,13 @@
 from apairo_preprocess.traversability import TraversabilityFromLabels, TraversabilityFromTrajectory
 from apairo_preprocess.ground import GroundSegmentationCSF, GroundSegmentationFromLabels, GroundSegmentationRANSAC
 from apairo_preprocess.priors import GroundHeightFromLabels, TrajectoryDistance
-from apairo_preprocess.odometry import GICPOdometry, KissICPOdometry
+from apairo_preprocess.odometry import (
+    GICPOdometry,
+    KissICPOdometry,
+    ReferencePoses,
+    pose_validity,
+    reference_poses,
+)
 from apairo_preprocess.pointcloud import VoxeliseCoords, VoxeliseLabels, VoxelisePointCloud, RemoveRobotPoints
 from apairo_preprocess.projection import BEVRasterisation, ImageMaskFromPointLabels, LidarCameraProjection, PointFeaturesFromImage, to_uint8_image
 
@@ -15,6 +21,9 @@ __all__ = [
     "TrajectoryDistance",
     "GICPOdometry",
     "KissICPOdometry",
+    "ReferencePoses",
+    "pose_validity",
+    "reference_poses",
     "VoxelisePointCloud",
     "VoxeliseLabels",
     "VoxeliseCoords",

{apairo_preprocess-0.2.0 → apairo_preprocess-0.4.0}/apairo_preprocess/ground/csf.py

@@ -35,10 +35,14 @@ def _run_csf(
     csf.params.class_threshold = class_threshold
     csf.params.interations = 500
 
+    if len(xyz) == 0:
+        return [], []
     csf.setPointCloud(xyz.tolist())
     ground_idx = CSF.VecInt()
     non_ground_idx = CSF.VecInt()
-    csf.do_filtering(ground_idx, non_ground_idx)
+    # exportCloth=False: by default CSF writes cloth_nodes.txt in the current
+    # directory, on every frame.
+    csf.do_filtering(ground_idx, non_ground_idx, False)
     return list(ground_idx), list(non_ground_idx)
 
 

{apairo_preprocess-0.2.0 → apairo_preprocess-0.4.0}/apairo_preprocess/ground/ransac.py

@@ -25,6 +25,8 @@ def _ransac_plane(
     n = len(xyz)
     best_inliers = -1
     best_normal = np.array([0.0, 0.0, 1.0])
+    if n < 3:  # no plane to fit: a horizontal one through what there is
+        return best_normal, (float(-np.median(xyz[:, 2])) if n else 0.0)
     best_d = float(-np.median(xyz[:, 2]))
 
     for _ in range(n_iter):

apairo_preprocess-0.4.0/apairo_preprocess/odometry/__init__.py

@@ -0,0 +1,15 @@
+from apairo_preprocess.odometry.gicp import GICPOdometry
+from apairo_preprocess.odometry.kissicp import KissICPOdometry
+from apairo_preprocess.odometry.reference import (
+    ReferencePoses,
+    pose_validity,
+    reference_poses,
+)
+
+__all__ = [
+    "GICPOdometry",
+    "KissICPOdometry",
+    "ReferencePoses",
+    "pose_validity",
+    "reference_poses",
+]

{apairo_preprocess-0.2.0 → apairo_preprocess-0.4.0}/apairo_preprocess/odometry/gicp.py

@@ -8,6 +8,10 @@ sessions) the accumulated drift will jump, but for single-session
 splits the poses are consistent.
 
 Output channel: ``gicp_poses``  (npys — one .npy per scan)
+
+This is an estimate: it returns a pose even where registration has lost
+track, with no signal of it. When the dataset ships its own poses, prefer
+:func:`~apairo_preprocess.reference_poses`.
 """
 
 from __future__ import annotations
@@ -68,6 +72,14 @@ class GICPOdometry(FramePreprocessor):
         self._prev_pcd = None
         self._T_accum = np.eye(4, dtype=np.float64)
 
+    def start_sequence(self) -> None:
+        """Forget the previous scan and the pose: the next scan is a new world
+        origin. Called by apairo's runner (>= 0.11) at the start of each
+        sequence of a root; without it a sequence continued the previous one's
+        pose and registered its first scan against an unrelated cloud."""
+        self._prev_pcd = None
+        self._T_accum = np.eye(4, dtype=np.float64)
+
     def __call__(self, sample: Sample) -> np.ndarray:
         pc = np.asarray(sample.data[self._lidar_key])
         xyz = pc[:, :3].astype(np.float64)

{apairo_preprocess-0.2.0 → apairo_preprocess-0.4.0}/apairo_preprocess/odometry/kissicp.py

@@ -6,6 +6,11 @@ and handles motion deskewing natively when per-point timestamps are available.
 
 Output channel: ``kissicp_poses``  (npys — one .npy per scan)
 
+This is an estimate: it returns a pose even where registration has lost
+track, with no signal of it, and its drift depends on the terrain (on
+TartanDrive 2.0, it drifted in vegetation where the INS did not). When the
+dataset ships its own poses, prefer :func:`~apairo_preprocess.reference_poses`.
+
 Typical usage::
 
     dataset.run_preprocess(
@@ -73,9 +78,19 @@ class KissICPOdometry(FramePreprocessor):
         cfg.data.min_range = min_range
         cfg.data.deskew = deskew
         cfg.mapping.voxel_size = float(voxel_size)
+        self._config = cfg
         self._kiss = KissICP(config=cfg)
         self._deskew = deskew
 
+    def start_sequence(self) -> None:
+        """Forget the map and the pose: the next scan is a new world origin.
+
+        Called by apairo's runner (>= 0.11) at the start of each sequence of a
+        root. Without it the first scan of a sequence was registered against
+        the previous sequence's map and continued its pose.
+        """
+        self._kiss = KissICP(config=self._config)
+
     def __call__(self, sample: Sample) -> np.ndarray:
         pc = np.asarray(sample.data[self._lidar_key], dtype=np.float64)
         xyz = pc[:, :3]

apairo_preprocess-0.4.0/apairo_preprocess/odometry/reference.py

@@ -0,0 +1,252 @@
+"""Poses delivered with a dataset, on the lidar clock.
+
+An INS, a GNSS-INS, or the trajectory a dataset ships with is a recorded
+measurement: turning it into one ``(4, 4)`` pose per scan is interpolation and
+composition, with no parameter that changes from sensor to sensor and no way
+to diverge. That is the point of this module, as opposed to
+:class:`~apairo_preprocess.KissICPOdometry` and
+:class:`~apairo_preprocess.GICPOdometry`, which *estimate* a trajectory and
+return a pose even where they have lost track.
+
+The rules are fixed and checkable by hand:
+
+- a scan gets a pose only when two pose rows bracket its time, at most
+  ``max_gap`` seconds apart; nothing is ever extrapolated;
+- a step between two consecutive scans faster than ``max_speed`` (or turning
+  faster than ``max_yaw_rate``) marks both of its scans invalid, since which
+  one is wrong cannot be told;
+- an invalid scan's pose is NaN, never a pose made up from its neighbours.
+
+The pose rows and the scans are on two clocks, which apairo's runner does not
+pair for a preprocessor yet: :func:`reference_poses` is therefore called
+directly, and its two outputs are written as channels with
+``apairo.ChannelWriter``.
+"""
+
+from __future__ import annotations
+
+import numpy as np
+from apairo_transform import PoseTo4x4
+from apairo_transform.interp import Se3Interp
+
+__all__ = ["ReferencePoses", "pose_validity", "reference_poses"]
+
+
+def _increasing(rows: np.ndarray, times: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
+    """Drop rows that repeat the previous timestamp (keeping the first); refuse
+    a clock that goes backwards."""
+    step = np.diff(times)
+    if np.any(step < 0):
+        k = int(np.flatnonzero(step < 0)[0])
+        raise ValueError(
+            f"pose timestamps go backwards at row {k + 1} "
+            f"({times[k + 1]!r} after {times[k]!r}); sort them first."
+        )
+    keep = np.concatenate([[True], step > 0])
+    return rows[keep], times[keep]
+
+
+def _yaw(poses: np.ndarray) -> np.ndarray:
+    return np.arctan2(poses[:, 1, 0], poses[:, 0, 0])
+
+
+def pose_validity(
+    poses: np.ndarray,
+    times: np.ndarray,
+    valid: np.ndarray | None = None,
+    *,
+    max_speed: float,
+    max_yaw_rate: float | None = None,
+) -> np.ndarray:
+    """``(N,)`` bool: the scans whose pose passes the physical bounds.
+
+    Args:
+        poses: ``(N, 4, 4)`` poses, one per scan, NaN where there is none.
+        times: ``(N,)`` scan times, seconds.
+        valid: Scans already known valid (default: those with a finite pose).
+        max_speed: The robot's top speed, m/s. No default: it is a property of
+            the robot, not of the data.
+        max_yaw_rate: Optional bound on the turn rate, rad/s.
+
+    Consecutive *valid* scans are compared. A step that exceeds a bound marks
+    both of its scans invalid. A step with no elapsed time (a scan repeated
+    under the same stamp, as some recordings do) has no speed and is not
+    checked; a time going backwards is an error.
+    """
+    poses = np.asarray(poses, dtype=np.float64)
+    times = np.asarray(times, dtype=np.float64)
+    if max_speed <= 0:
+        raise ValueError(f"max_speed must be positive, got {max_speed}")
+    finite = np.all(np.isfinite(poses.reshape(len(poses), -1)), axis=1)
+    out = finite if valid is None else (np.asarray(valid, dtype=bool) & finite)
+    idx = np.flatnonzero(out)
+    if len(idx) < 2:
+        return out
+    dt = np.diff(times[idx])
+    if np.any(dt < 0):
+        raise ValueError("scan times go backwards; sort them first")
+    timed = dt > 0
+    safe = np.where(timed, dt, 1.0)
+    dist = np.linalg.norm(np.diff(poses[idx, :3, 3], axis=0), axis=1)
+    bad = timed & (dist / safe > max_speed)
+    if max_yaw_rate is not None:
+        dyaw = np.abs(np.angle(np.exp(1j * np.diff(_yaw(poses[idx])))))
+        bad |= timed & (dyaw / safe > max_yaw_rate)
+    out = out.copy()
+    out[idx[:-1][bad]] = False
+    out[idx[1:][bad]] = False
+    return out
+
+
+def reference_poses(
+    rows: np.ndarray,
+    row_times: np.ndarray,
+    scan_times: np.ndarray,
+    *,
+    clock_offset: float = 0.0,
+    extrinsic: np.ndarray | None = None,
+    max_gap: float = 0.1,
+    max_speed: float | None = None,
+    max_yaw_rate: float | None = None,
+    relative: bool = True,
+) -> tuple[np.ndarray, np.ndarray]:
+    """One pose per scan from delivered pose rows.
+
+    Args:
+        rows: ``(M, >=7)`` rows ``[x, y, z, qx, qy, qz, qw, ...]``: the body's
+            pose in the world frame. Extra columns (a twist) are ignored.
+        row_times: ``(M,)`` row times, seconds. A row repeating the previous
+            time is dropped; a time going backwards is an error.
+        scan_times: ``(N,)`` scan times, seconds.
+        clock_offset: Added to the scan times before looking the poses up:
+            the pose source's clock minus the lidar's, as measured once for
+            the rig.
+        extrinsic: ``(4, 4)`` pose of the sensor in the body frame, composed on
+            the right (``T_world_sensor = T_world_body @ extrinsic``). ``None``
+            when the sensor is the body frame.
+        max_gap: Largest time between the two rows bracketing a scan, seconds.
+        max_speed: If given, :func:`pose_validity` is applied with it.
+        max_yaw_rate: Passed to :func:`pose_validity`.
+        relative: Express every pose relative to the first valid one, in
+            float64. Positions recorded in a global frame (UTM: ~1e6 m) keep
+            their millimetres only this way once a consumer casts to float32.
+
+    Returns:
+        ``(poses, valid)``: ``(N, 4, 4)`` float64, NaN where ``valid`` is False,
+        and ``(N,)`` bool.
+    """
+    rows = np.asarray(rows, dtype=np.float64)
+    row_times = np.asarray(row_times, dtype=np.float64)
+    scan_times = np.asarray(scan_times, dtype=np.float64)
+    if rows.ndim != 2 or rows.shape[1] < 7:
+        raise ValueError(
+            f"rows must be (M, >=7) [x y z qx qy qz qw ...], got {rows.shape}"
+        )
+    if len(rows) != len(row_times):
+        raise ValueError(f"{len(rows)} rows but {len(row_times)} row times")
+    if max_gap <= 0:
+        raise ValueError(f"max_gap must be positive, got {max_gap}")
+    rows, row_times = _increasing(rows[:, :7], row_times)
+
+    n = len(scan_times)
+    poses = np.full((n, 4, 4), np.nan)
+    valid = np.zeros(n, dtype=bool)
+    if len(rows) == 0 or n == 0:
+        return poses, valid
+
+    tq = scan_times + clock_offset
+    hi = np.searchsorted(row_times, tq, side="left")
+    exact = (hi < len(row_times)) & (
+        row_times[np.minimum(hi, len(row_times) - 1)] == tq
+    )
+    interp = Se3Interp()
+    to4 = PoseTo4x4()
+    for i in range(n):
+        if exact[i]:
+            pose7 = rows[hi[i]]
+        else:
+            j = int(hi[i])
+            if j == 0 or j == len(row_times):
+                continue  # outside the recorded span: no extrapolation
+            t0, t1 = row_times[j - 1], row_times[j]
+            if t1 - t0 > max_gap:
+                continue
+            pose7 = interp(tq[i], t0, rows[j - 1], t1, rows[j])
+        poses[i] = to4(pose7)
+        valid[i] = True
+
+    if extrinsic is not None:
+        extrinsic = np.asarray(extrinsic, dtype=np.float64)
+        if extrinsic.shape != (4, 4):
+            raise ValueError(f"extrinsic must be (4, 4), got {extrinsic.shape}")
+        poses[valid] = poses[valid] @ extrinsic
+    if max_speed is not None:
+        valid = pose_validity(
+            poses, scan_times, valid, max_speed=max_speed, max_yaw_rate=max_yaw_rate
+        )
+        poses[~valid] = np.nan
+    if relative and valid.any():
+        first = poses[int(np.flatnonzero(valid)[0])]
+        poses[valid] = np.linalg.inv(first) @ poses[valid]
+    return poses, valid
+
+
+class ReferencePoses:
+    """:func:`reference_poses` with its parameters fixed once for a rig.
+
+    Example::
+
+        ins = ReferencePoses(clock_offset=-0.05, max_gap=0.1, max_speed=20.0)
+        poses, valid = ins(rows, row_times, scan_times)
+        # then write both with apairo.ChannelWriter, on the lidar clock
+    """
+
+    def __init__(
+        self,
+        *,
+        clock_offset: float = 0.0,
+        extrinsic: np.ndarray | None = None,
+        max_gap: float = 0.1,
+        max_speed: float | None = None,
+        max_yaw_rate: float | None = None,
+        relative: bool = True,
+    ) -> None:
+        self.clock_offset = float(clock_offset)
+        self.extrinsic = (
+            None if extrinsic is None else np.asarray(extrinsic, dtype=np.float64)
+        )
+        self.max_gap = float(max_gap)
+        self.max_speed = max_speed
+        self.max_yaw_rate = max_yaw_rate
+        self.relative = relative
+
+    def __call__(
+        self, rows: np.ndarray, row_times: np.ndarray, scan_times: np.ndarray
+    ) -> tuple[np.ndarray, np.ndarray]:
+        return reference_poses(
+            rows,
+            row_times,
+            scan_times,
+            clock_offset=self.clock_offset,
+            extrinsic=self.extrinsic,
+            max_gap=self.max_gap,
+            max_speed=self.max_speed,
+            max_yaw_rate=self.max_yaw_rate,
+            relative=self.relative,
+        )
+
+    def get_config(self) -> dict:
+        return {
+            "clock_offset": self.clock_offset,
+            "extrinsic": None if self.extrinsic is None else self.extrinsic.tolist(),
+            "max_gap": self.max_gap,
+            "max_speed": self.max_speed,
+            "max_yaw_rate": self.max_yaw_rate,
+            "relative": self.relative,
+        }
+
+    def __repr__(self) -> str:
+        return (
+            f"ReferencePoses(clock_offset={self.clock_offset}, max_gap={self.max_gap}, "
+            f"max_speed={self.max_speed})"
+        )