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

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

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: apairo-preprocess
-Version: 0.3.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,37 @@ 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
@@ -201,6 +256,16 @@ Goose3DDataset.run_preprocess(
 # 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.3.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,37 @@ 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
@@ -169,6 +224,16 @@ Goose3DDataset.run_preprocess(
 # 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.3.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.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.3.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

{apairo_preprocess-0.3.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(

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})"
+        )

{apairo_preprocess-0.3.0 → apairo_preprocess-0.4.0}/apairo_preprocess/traversability/from_trajectory.py

@@ -106,6 +106,16 @@ class TraversabilityFromTrajectory(SequencePreprocessor):
                                ``np.inf``. A callable ``(N, 3) -> (N,)`` is
                                accepted too.
         output_key:            Override the default output channel name ``"trav_traj"``.
+        valid_key:             Channel holding one bool per scan: whether its pose
+                               can be trusted (e.g. ``reference_poses``' second
+                               output). An invalid scan cuts the trajectory as
+                               ``sequence_gap`` does -- no look-ahead across it --
+                               and none of its points is traversable. Without it,
+                               a non-finite pose is an error.
+        unknown_label:         Label of every point of an invalid scan, e.g. 255
+                               for "unknown". ``None`` (default) labels them 0,
+                               which a consumer cannot tell from "not
+                               traversable".
     """
 
     output_key: ClassVar[str] = "trav_traj"
@@ -129,6 +139,8 @@ class TraversabilityFromTrajectory(SequencePreprocessor):
         near_exclusion_norm: float = np.inf,
         output_key: str | None = None,
         pose_height: float = 0.0,
+        valid_key: str | None = None,
+        unknown_label: int | None = None,
     ) -> None:
         if (robot_length is None) != (robot_width is None):
             raise ValueError("robot_length and robot_width must be given together")
@@ -161,8 +173,15 @@ class TraversabilityFromTrajectory(SequencePreprocessor):
             else None
         )
 
-        self.input_keys = [lidar_key, poses_key]
-        self.sources = [lidar_key, poses_key]
+        if unknown_label is not None and not 0 <= int(unknown_label) <= 255:
+            raise ValueError(f"unknown_label must fit in uint8, got {unknown_label}")
+        if unknown_label is not None and int(unknown_label) in (0, 1):
+            raise ValueError("unknown_label must differ from the labels 0 and 1")
+        self._valid_key = valid_key
+        self._unknown = None if unknown_label is None else int(unknown_label)
+        keys = [lidar_key, poses_key] + ([valid_key] if valid_key is not None else [])
+        self.input_keys = keys
+        self.sources = list(keys)
         if output_key is not None:
             self.output_key = output_key
 
@@ -181,15 +200,33 @@ class TraversabilityFromTrajectory(SequencePreprocessor):
                 f"Got {poses.shape}"
             )
 
+        pose_finite = np.all(np.isfinite(poses.reshape(n, -1)), axis=1)
+        if self._valid_key is None:
+            if not pose_finite.all():
+                raise ValueError(
+                    f"{int((~pose_finite).sum())} pose(s) are not finite. Pass the "
+                    f"validity of each pose as valid_key (reference_poses gives "
+                    f"it), so that those scans are left out."
+                )
+            valid = pose_finite
+        else:
+            valid = np.array(
+                [bool(np.asarray(s.data[self._valid_key])) for s in all_samples]
+            ) & pose_finite
+
         # Sequence boundary detection — done here where poses are available.
+        # A step is broken by a gap larger than sequence_gap or by an invalid
+        # scan at either end; NaN distances (invalid poses) are broken too.
         positions = poses[:, :3, 3]
         dists = np.linalg.norm(np.diff(positions, axis=0), axis=1)
-        boundaries = np.where(dists > self._sequence_gap)[0]
+        broken = ~(dists <= self._sequence_gap) | ~valid[:-1] | ~valid[1:]
+        boundaries = np.where(broken)[0]
         ends = np.concatenate([boundaries, [n - 1]])
         seq_end = ends[np.searchsorted(ends, np.arange(n))]
 
-        # Cumulative path length, for the forward_distance cap.
-        path = np.concatenate([[0.0], np.cumsum(dists)])
+        # Cumulative path length, for the forward_distance cap. A broken step
+        # adds nothing: no look-ahead crosses it.
+        path = np.concatenate([[0.0], np.cumsum(np.where(broken, 0.0, dists))])
 
         if self._oriented:
             # Yaw from each pose's own rotation matrix -- robust where the robot is
@@ -202,6 +239,10 @@ class TraversabilityFromTrajectory(SequencePreprocessor):
         results = []
         for idx, sample in enumerate(all_samples):
             pc = np.asarray(sample.data[self._lidar_key])
+            if not valid[idx]:
+                fill = 0 if self._unknown is None else self._unknown
+                results.append(np.full(len(pc), fill, dtype=np.uint8))
+                continue
             xyz_all = pc[:, :3].astype(np.float64)
             # A return without a position (NaN or inf) is never traversable; it
             # used to abort the whole sequence in the nearest-pose search.

{apairo_preprocess-0.3.0 → apairo_preprocess-0.4.0}/apairo_preprocess.egg-info/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: apairo-preprocess
-Version: 0.3.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,37 @@ 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
@@ -201,6 +256,16 @@ Goose3DDataset.run_preprocess(
 # 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.3.0 → apairo_preprocess-0.4.0}/apairo_preprocess.egg-info/SOURCES.txt

@@ -14,6 +14,7 @@ apairo_preprocess/ground/ransac.py
 apairo_preprocess/odometry/__init__.py
 apairo_preprocess/odometry/gicp.py
 apairo_preprocess/odometry/kissicp.py
+apairo_preprocess/odometry/reference.py
 apairo_preprocess/pointcloud/__init__.py
 apairo_preprocess/pointcloud/robot_filter.py
 apairo_preprocess/pointcloud/voxelise.py
@@ -37,6 +38,7 @@ tests/test_kissicp_odometry.py
 tests/test_lidar_camera_projection.py
 tests/test_point_features_from_image.py
 tests/test_projection_pipeline.py
+tests/test_reference_poses.py
 tests/test_remove_robot_points.py
 tests/test_traversability_from_labels.py
 tests/test_traversability_from_trajectory.py

{apairo_preprocess-0.3.0 → apairo_preprocess-0.4.0}/pyproject.toml

@@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta"
 
 [project]
 name = "apairo-preprocess"
-version = "0.3.0"
+version = "0.4.0"
 description = "Preprocessing for apairo datasets: odometry, ground segmentation, projection, traversability labels"
 readme = "README.md"
 authors = [

apairo_preprocess-0.4.0/tests/test_reference_poses.py

@@ -0,0 +1,288 @@
+"""Delivered poses on the lidar clock: interpolation, validity, labelling.
+
+Each test states the physical expectation first.
+"""
+
+import numpy as np
+import pytest
+from apairo.core.sample import Sample
+from scipy.spatial.transform import Rotation
+
+from apairo_preprocess import (
+    ReferencePoses,
+    TraversabilityFromTrajectory,
+    pose_validity,
+    reference_poses,
+)
+
+
+def _rows(t, x, y, z, yaw):
+    """Rows [x y z qx qy qz qw] of a body yawing in the plane."""
+    q = Rotation.from_euler("z", np.asarray(yaw, dtype=float).reshape(-1, 1)).as_quat()
+    return np.column_stack([x, y, z, q])
+
+
+def _true_pose(x, y, z, yaw):
+    T = np.eye(4)
+    T[:3, :3] = Rotation.from_euler("z", yaw).as_matrix()
+    T[:3, 3] = [x, y, z]
+    return T
+
+
+# ------------------------------------------------------------------ interpolation
+
+
+def test_a_straight_line_is_interpolated_exactly():
+    # 5 m/s along x, rows at 50 Hz, scans at 10 Hz in between the rows.
+    t = np.arange(0, 2.0001, 0.02)
+    rows = _rows(t, 5 * t, 0 * t, 0 * t, 0 * t)
+    scans = np.arange(0.005, 1.9, 0.1)
+    poses, valid = reference_poses(rows, t, scans, relative=False)
+    assert valid.all()
+    np.testing.assert_allclose(poses[:, 0, 3], 5 * scans, atol=1e-9)
+    np.testing.assert_allclose(
+        poses[:, :3, :3], np.tile(np.eye(3), (len(scans), 1, 1)), atol=1e-12
+    )
+
+
+def test_a_turn_is_interpolated_on_position_and_heading():
+    # Constant yaw rate: both position (lerp between close rows) and heading
+    # (slerp) match the truth at mid-row times.
+    t = np.arange(0, 2.0001, 0.02)
+    yaw = 0.5 * t
+    rows = _rows(t, np.cos(yaw), np.sin(yaw), 0 * t, yaw)
+    scans = np.array([0.31, 0.97, 1.53])
+    poses, valid = reference_poses(rows, t, scans, relative=False)
+    assert valid.all()
+    got_yaw = np.arctan2(poses[:, 1, 0], poses[:, 0, 0])
+    np.testing.assert_allclose(got_yaw, 0.5 * scans, atol=1e-9)
+    # lerp of a circle's chord: error below r * (dtheta)^2 / 8 = 1.25e-5 m
+    np.testing.assert_allclose(poses[:, 0, 3], np.cos(0.5 * scans), atol=2e-5)
+
+
+def test_the_clock_offset_shifts_the_lookup():
+    # The pose clock runs 0.05 s behind the lidar's: the pose of a scan at t
+    # is the row at t - 0.05.
+    t = np.arange(0, 2.0001, 0.02)
+    rows = _rows(t, 5 * t, 0 * t, 0 * t, 0 * t)
+    scans = np.array([0.5, 1.0])
+    poses, _ = reference_poses(rows, t, scans, clock_offset=-0.05, relative=False)
+    np.testing.assert_allclose(poses[:, 0, 3], 5 * (scans - 0.05), atol=1e-9)
+
+
+def test_utm_scale_positions_keep_their_millimetres_relative_to_the_first():
+    # Northing 4.48e6 m: relative to the first scan, in float64, the 1 mm
+    # steps survive; a float32 cast of the absolute value would not.
+    t = np.arange(0, 1.0001, 0.02)
+    x = 4.48e6 + 0.001 * np.arange(len(t))
+    rows = _rows(t, x, -6.03e5 + 0 * t, 300 + 0 * t, 0 * t)
+    poses, valid = reference_poses(rows, t, t[:10])
+    assert valid.all()
+    np.testing.assert_allclose(poses[:, 0, 3], 0.001 * np.arange(10), atol=1e-7)
+    np.testing.assert_allclose(poses[0], np.eye(4), atol=1e-12)
+    assert np.ptp(poses[:, 0, 3].astype(np.float32)) > 0.008
+
+
+def test_an_extrinsic_is_composed_on_the_right():
+    # Body at (10, 0, 0) facing +y; sensor 1 m ahead of the body, 2 m up:
+    # in the world it sits at (10, 1, 2), facing +y.
+    t = np.array([0.0, 0.1])
+    rows = _rows(
+        t, np.array([10.0, 10.0]), np.zeros(2), np.zeros(2), np.full(2, np.pi / 2)
+    )
+    sensor = np.eye(4)
+    sensor[:3, 3] = [1.0, 0.0, 2.0]
+    poses, _ = reference_poses(
+        rows, t, np.array([0.05]), extrinsic=sensor, relative=False
+    )
+    np.testing.assert_allclose(poses[0, :3, 3], [10.0, 1.0, 2.0], atol=1e-12)
+    np.testing.assert_allclose(
+        poses[0, :3, :3], Rotation.from_euler("z", np.pi / 2).as_matrix(), atol=1e-12
+    )
+
+
+# ---------------------------------------------------------------------- validity
+
+
+def test_scans_inside_a_hole_in_the_rows_have_no_pose():
+    # Rows stop for 0.5 s: the scans inside are invalid, not interpolated
+    # across the hole; their pose is NaN.
+    t = np.concatenate([np.arange(0, 1.0, 0.02), np.arange(1.5, 2.5, 0.02)])
+    rows = _rows(t, 5 * t, 0 * t, 0 * t, 0 * t)
+    scans = np.array([0.5, 1.2, 1.3, 2.0])
+    poses, valid = reference_poses(rows, t, scans, max_gap=0.1)
+    assert valid.tolist() == [True, False, False, True]
+    assert np.isnan(poses[1:3]).all() and np.isfinite(poses[[0, 3]]).all()
+
+
+def test_nothing_is_extrapolated_outside_the_recorded_span():
+    t = np.arange(1.0, 2.0001, 0.02)
+    rows = _rows(t, t, 0 * t, 0 * t, 0 * t)
+    _, valid = reference_poses(rows, t, np.array([0.9, 1.5, 2.1]))
+    assert valid.tolist() == [False, True, False]
+
+
+def test_a_repeated_row_is_dropped_and_a_clock_going_back_is_refused():
+    t = np.array([0.0, 0.02, 0.02, 0.04])
+    rows = _rows(t, t, 0 * t, 0 * t, 0 * t)
+    _, valid = reference_poses(rows, t, np.array([0.01, 0.03]))
+    assert valid.all()
+    with pytest.raises(ValueError, match="backwards"):
+        reference_poses(rows, np.array([0.0, 0.02, 0.01, 0.04]), np.array([0.01]))
+
+
+def test_a_jump_beyond_the_top_speed_invalidates_both_scans_of_the_step():
+    # 5 m/s, except one row 5 m off: the step into it and the step out of it
+    # both exceed 15 m/s; the scan before, the jumped one and the one after
+    # are invalid.
+    t = np.arange(0, 2.0001, 0.1)
+    x = 5 * t
+    x[10] += 5.0
+    rows = _rows(t, x, 0 * t, 0 * t, 0 * t)
+    poses, valid = reference_poses(rows, t, t, max_speed=15.0)
+    assert np.flatnonzero(~valid).tolist() == [9, 10, 11]
+    assert np.isnan(poses[~valid]).all()
+
+
+def test_pose_validity_needs_a_positive_top_speed_and_checks_turns():
+    t = np.arange(0, 1.0001, 0.1)
+    yaw = np.zeros_like(t)
+    yaw[5] = 1.0  # a 1 rad flick in 0.1 s
+    poses = np.stack([_true_pose(ti, 0, 0, a) for ti, a in zip(t, yaw)])
+    assert pose_validity(poses, t, max_speed=15.0).all()
+    assert np.flatnonzero(
+        ~pose_validity(poses, t, max_speed=15.0, max_yaw_rate=2.0)
+    ).tolist() == [4, 5, 6]
+    with pytest.raises(ValueError):
+        pose_validity(poses, t, max_speed=0.0)
+
+
+def test_a_scan_repeated_under_the_same_stamp_is_not_a_jump():
+    # Real recordings repeat a scan under the same stamp: no time elapses,
+    # there is no speed to check, and both copies keep their pose.
+    t = np.arange(0, 1.0001, 0.02)
+    rows = _rows(t, 5 * t, 0 * t, 0 * t, 0 * t)
+    scans = np.array([0.1, 0.2, 0.2, 0.3])
+    poses, valid = reference_poses(rows, t, scans, max_speed=15.0)
+    assert valid.all()
+    np.testing.assert_array_equal(poses[1], poses[2])
+    with pytest.raises(ValueError, match="backwards"):
+        pose_validity(poses, np.array([0.1, 0.2, 0.15, 0.3]), max_speed=15.0)
+
+
+def test_reference_poses_class_keeps_its_configuration():
+    rp = ReferencePoses(clock_offset=-0.05, max_gap=0.1, max_speed=20.0)
+    assert rp.get_config()["clock_offset"] == -0.05
+    t = np.arange(0, 1.0001, 0.02)
+    rows = _rows(t, t, 0 * t, 0 * t, 0 * t)
+    a = rp(rows, t, np.array([0.5]))
+    b = reference_poses(rows, t, np.array([0.5]), clock_offset=-0.05, max_speed=20.0)
+    np.testing.assert_array_equal(a[0], b[0])
+
+
+# ----------------------------------------------------------------- labelling
+
+
+def _frames(poses, valid, xyz, valid_key="valid"):
+    xyz = np.asarray(xyz, dtype=np.float32)
+    return iter(
+        [
+            Sample(data={"lidar": xyz, "poses": p, valid_key: np.bool_(v)})
+            for p, v in zip(poses, valid)
+        ]
+    )
+
+
+def _line(n, step):
+    poses = np.tile(np.eye(4), (n, 1, 1))
+    poses[:, 0, 3] = np.arange(n) * step
+    return poses
+
+
+def test_a_non_finite_pose_without_valid_key_is_an_error():
+    poses = _line(4, 1.0)
+    poses[2] = np.nan
+    frames = iter(
+        [
+            Sample(data={"lidar": np.zeros((1, 3), np.float32), "poses": p})
+            for p in poses
+        ]
+    )
+    with pytest.raises(ValueError, match="valid_key"):
+        TraversabilityFromTrajectory(robot_radius=0.5)(frames)
+
+
+def test_an_invalid_scan_cuts_the_look_ahead_and_is_marked_unknown():
+    # Straight line, 1 m steps, a point 2 m ahead of each scan, on the path.
+    # Scan 1 is invalid. From scan 0 the point lies on scan 2, but the
+    # look-ahead stops at scan 1: not traversable. Scan 1's own points are
+    # unknown (255). From scan 2 the point lies on scan 4: traversable.
+    poses = _line(6, 1.0)
+    poses[1] = np.nan
+    valid = [True, False, True, True, True, True]
+    xyz = [[2.0, 0.0, 0.0]]
+    lab = TraversabilityFromTrajectory(
+        robot_radius=0.3, valid_key="valid", unknown_label=255
+    )(_frames(poses, valid, xyz))
+    assert lab[0].tolist() == [0]
+    assert lab[1].tolist() == [255]
+    assert lab[2].tolist() == [1]
+
+
+def test_without_unknown_label_an_invalid_scan_is_zero():
+    poses = _line(4, 1.0)
+    poses[1] = np.nan
+    lab = TraversabilityFromTrajectory(robot_radius=0.3, valid_key="valid")(
+        _frames(poses, [True, False, True, True], [[1.0, 0.0, 0.0]])
+    )
+    assert lab[1].tolist() == [0]
+
+
+def test_unknown_label_must_not_collide_with_the_labels():
+    for bad in (0, 1, 256):
+        with pytest.raises(ValueError):
+            TraversabilityFromTrajectory(valid_key="valid", unknown_label=bad)
+
+
+def test_valid_key_is_an_input_and_a_source():
+    p = TraversabilityFromTrajectory(valid_key="ins_valid")
+    assert p.input_keys == ["lidar", "poses", "ins_valid"]
+    assert p.sources == ["lidar", "poses", "ins_valid"]
+
+
+def test_end_to_end_delivered_rows_give_the_labels_of_the_true_poses():
+    # A drive with a turn, recorded by a 50 Hz INS 1.2 m above the ground,
+    # scans at 10 Hz on a lidar clock 0.05 s ahead: labelling the poses
+    # rebuilt from the rows gives exactly the labels of the true poses.
+    t = np.arange(0, 12.0001, 0.02)
+    yaw = np.where(t < 6, 0.0, 0.3 * (t - 6))
+    x = np.cumsum(np.r_[0, 3.0 * np.cos(yaw[:-1]) * 0.02])
+    y = np.cumsum(np.r_[0, 3.0 * np.sin(yaw[:-1]) * 0.02])
+    rows = _rows(t, x, y, np.full_like(t, 1.2), yaw)
+    scans = np.arange(0.1, 11.0, 0.1)
+    poses, valid = reference_poses(rows, t, scans, clock_offset=-0.05, max_speed=10.0)
+    assert valid.all()
+
+    ts = scans - 0.05
+    truth = np.stack(
+        [
+            _true_pose(
+                np.interp(s, t, x), np.interp(s, t, y), 1.2, np.interp(s, t, yaw)
+            )
+            for s in ts
+        ]
+    )
+    truth = np.linalg.inv(truth[0]) @ truth
+    rng = np.random.default_rng(0)
+    ground = np.column_stack(
+        [rng.uniform(0, 12, 400), rng.uniform(-2, 2, 400), np.full(400, -1.2)]
+    ).astype(np.float32)
+    kw = dict(robot_radius=0.6, pose_height=1.2, forward_distance=15.0)
+    a = TraversabilityFromTrajectory(**kw)(
+        iter([Sample(data={"lidar": ground, "poses": p}) for p in poses])
+    )
+    b = TraversabilityFromTrajectory(**kw)(
+        iter([Sample(data={"lidar": ground, "poses": p}) for p in truth])
+    )
+    assert int(np.asarray(b).sum()) > 0
+    np.testing.assert_array_equal(np.asarray(a), np.asarray(b))

apairo_preprocess-0.3.0/apairo_preprocess/odometry/__init__.py

@@ -1,4 +0,0 @@
-from apairo_preprocess.odometry.gicp import GICPOdometry
-from apairo_preprocess.odometry.kissicp import KissICPOdometry
-
-__all__ = ["GICPOdometry", "KissICPOdometry"]