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

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

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: apairo-preprocess
-Version: 0.2.0
+Version: 0.3.0
 Summary: Preprocessing for apairo datasets: odometry, ground segmentation, projection, traversability labels
 Author-email: Augustin Bresset <augustin.bresset@gmail.com>
 License: MIT
@@ -183,13 +183,19 @@ SemanticKittiDataset.run_preprocess(
 
 Requires poses to be computed first (e.g. with `KissICPOdometry`).
 
+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
 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)

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

@@ -151,13 +151,19 @@ SemanticKittiDataset.run_preprocess(
 
 Requires poses to be computed first (e.g. with `KissICPOdometry`).
 
+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
 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)

{apairo_preprocess-0.2.0 → apairo_preprocess-0.3.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.3.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.2.0 → apairo_preprocess-0.3.0}/apairo_preprocess/odometry/gicp.py

@@ -68,6 +68,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.3.0}/apairo_preprocess/odometry/kissicp.py

@@ -73,9 +73,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.2.0 → apairo_preprocess-0.3.0}/apairo_preprocess/pointcloud/voxelise.py

@@ -47,9 +47,14 @@ def _filter_and_quantize(
         inverse:       Per-point voxel index array, shape (N_kept,).
         n_voxels:      Number of occupied voxels.
     """
-    mask: np.ndarray | None = None
+    # A return without a position (NaN or inf) belongs to no voxel: it used to
+    # become a voxel at the int32 minimum, with a NaN row in the output.
+    mask: np.ndarray | None = np.all(np.isfinite(xyz), axis=1)
     if max_range is not None:
-        mask = np.linalg.norm(xyz, axis=1) < max_range
+        mask &= np.linalg.norm(xyz, axis=1) < max_range
+    if mask.all():
+        mask = None
+    else:
         xyz = xyz[mask]
 
     if len(xyz) == 0:
@@ -76,6 +81,8 @@ class VoxelisePointCloud(FramePreprocessor):
         max_range:   Discard points farther than this distance from the sensor
                      origin (metres).  ``None`` keeps all points.
         reduction:   How to pick the representative point per voxel.
+        seed:        Seed of ``reduction="random"``. With a seed, the same scan
+                     gives the same points on every run; ``None`` draws anew.
         output_key:  Override the default output channel name ``"voxelised"``.
     """
 
@@ -91,11 +98,13 @@ class VoxelisePointCloud(FramePreprocessor):
         max_range: float | None = None,
         reduction: _Reduction = "centroid",
         output_key: str | None = None,
+        seed: int | None = None,
     ) -> None:
         if voxel_size <= 0:
             raise ValueError(f"voxel_size must be positive, got {voxel_size}")
         if max_range is not None and max_range <= 0:
             raise ValueError(f"max_range must be positive, got {max_range}")
+        self.seed = seed
         if reduction not in ("centroid", "first", "random"):
             raise ValueError(
                 f"reduction must be 'centroid', 'first', or 'random', got {reduction!r}"
@@ -134,13 +143,14 @@ class VoxelisePointCloud(FramePreprocessor):
             return out
 
         if self._reduction == "first":
-            out = np.empty((n_voxels, pc.shape[1]), dtype=np.float64)
-            # assign in reverse so the first occurrence wins
-            out[inverse] = pc
-            return out
+            # The lowest input index of each voxel (a plain `out[inverse] = pc`
+            # keeps the last point written, not the first).
+            first = np.full(n_voxels, len(pc), dtype=np.int64)
+            np.minimum.at(first, inverse, np.arange(len(pc)))
+            return pc[first]
 
         # random: shuffle so a random point ends up last (and thus selected)
-        rng = np.random.default_rng()
+        rng = np.random.default_rng(self.seed)
         chosen = np.empty(n_voxels, dtype=np.int64)
         perm = rng.permutation(len(pc))
         chosen[inverse[perm]] = perm
@@ -212,16 +222,28 @@ class VoxeliseLabels(FramePreprocessor):
         if n_voxels == 0:
             return np.empty(0, dtype=labels.dtype)
 
+        # Both aggregations go through one sort of the (voxel, label) pairs,
+        # so they hold for any label values: negative ids (an ignore label of
+        # -1), and large ones (SemanticKITTI's instance << 16 | class), which a
+        # table of n_voxels x max_label counters cannot.
+        order = np.lexsort((labels, inverse))
+        voxel, label = inverse[order], labels[order]
+        out = np.empty(n_voxels, dtype=labels.dtype)
         if self._aggregation == "max":
-            out = np.zeros(n_voxels, dtype=labels.dtype)
-            np.maximum.at(out, inverse, labels)
+            last = np.r_[voxel[1:] != voxel[:-1], True]  # highest label of each voxel
+            out[voxel[last]] = label[last]
             return out
 
-        # majority: encode (voxel_idx, label) as a single int and bincount
-        n_classes = int(labels.max()) + 1
-        keys = inverse.astype(np.int64) * n_classes + labels.astype(np.int64)
-        pair_counts = np.bincount(keys, minlength=n_voxels * n_classes)
-        return pair_counts.reshape(n_voxels, n_classes).argmax(axis=1).astype(labels.dtype)
+        # majority: runs of equal (voxel, label), the longest per voxel; ties
+        # go to the smallest label.
+        start = np.r_[True, (voxel[1:] != voxel[:-1]) | (label[1:] != label[:-1])]
+        starts = np.flatnonzero(start)
+        counts = np.diff(np.r_[starts, len(voxel)])
+        run_voxel, run_label = voxel[starts], label[starts]
+        best = np.lexsort((run_label, -counts, run_voxel))
+        pick = best[np.r_[True, run_voxel[best][1:] != run_voxel[best][:-1]]]
+        out[run_voxel[pick]] = run_label[pick]
+        return out
 
 
 class VoxeliseCoords(FramePreprocessor):

{apairo_preprocess-0.2.0 → apairo_preprocess-0.3.0}/apairo_preprocess/priors/ground_height.py

@@ -49,6 +49,8 @@ class GroundHeightFromLabels(FramePreprocessor):
     def __call__(self, sample: Sample) -> np.ndarray:
         xyz = np.asarray(sample.data[self._voxelised_key], dtype=np.float64)[:, :3]
         labels = np.asarray(sample.data[self._ground_key])
+        if len(xyz) == 0:
+            return np.empty(0, dtype=np.float32)
         ground_pts = xyz[labels == 0]
 
         if len(ground_pts) == 0:

{apairo_preprocess-0.2.0 → apairo_preprocess-0.3.0}/apairo_preprocess/projection/bev.py

@@ -29,17 +29,33 @@ from apairo.core.preprocessor import FramePreprocessor
 from apairo.core.sample import Sample
 
 
+def _grid_size(bounds, resolution) -> tuple[int, int]:
+    """``(h, w)`` of the grid covering *bounds*: the last row or column is cut
+    by the bound when the extent is not a multiple of the resolution. (Rounding
+    the count dropped points inside the bounds, or took in points outside.)"""
+    xmin, xmax, ymin, ymax = bounds
+    w = int(np.ceil(round((xmax - xmin) / resolution, 9)))
+    h = int(np.ceil(round((ymax - ymin) / resolution, 9)))
+    return h, w
+
+
 def _rasterise(points, values, *, bounds, resolution, reduce, background):
     xmin, xmax, ymin, ymax = bounds
-    w = int(round((xmax - xmin) / resolution))
-    h = int(round((ymax - ymin) / resolution))
+    h, w = _grid_size(bounds, resolution)
 
-    xy = np.asarray(points)[:, :2]
+    xy = np.asarray(points)[:, :2].astype(np.float64)
     vals = np.asarray(values, dtype=np.float64)
 
-    col = np.floor((xy[:, 0] - xmin) / resolution).astype(np.int64)
-    row = np.floor((ymax - xy[:, 1]) / resolution).astype(np.int64)
-    inside = (col >= 0) & (col < w) & (row >= 0) & (row < h)
+    # The tiny offset keeps a point lying exactly on a cell edge in its cell:
+    # 0.3 / 0.1 is 2.9999999999999996 in floating point.
+    col = np.floor((xy[:, 0] - xmin) / resolution + 1e-9).astype(np.int64)
+    row = np.floor((ymax - xy[:, 1]) / resolution + 1e-9).astype(np.int64)
+    inside = (
+        (xy[:, 0] >= xmin) & (xy[:, 0] < xmax) & (xy[:, 1] > ymin) & (xy[:, 1] <= ymax)
+        & (col >= 0) & (col < w) & (row >= 0) & (row < h)
+        # A value that is not a number says nothing about its cell.
+        & np.isfinite(vals)
+    )
     cell = row[inside] * w + col[inside]
     vals = vals[inside]
 
@@ -51,7 +67,8 @@ def _rasterise(points, values, *, bounds, resolution, reduce, background):
         # no point touched.
         acc = np.full(h * w, -np.inf)
         np.maximum.at(acc, cell, vals)
-        hit = np.isfinite(acc)
+        hit = np.zeros(h * w, dtype=bool)
+        hit[cell] = True
         grid[hit] = acc[hit]
     elif reduce == "last":
         grid[cell] = vals
@@ -125,10 +142,7 @@ class BEVRasterisation(FramePreprocessor):
     @property
     def shape(self) -> tuple[int, int]:
         """``(height, width)`` of the output grid, from ``bounds``/``resolution``."""
-        xmin, xmax, ymin, ymax = self._bounds
-        w = int(round((xmax - xmin) / self._resolution))
-        h = int(round((ymax - ymin) / self._resolution))
-        return h, w
+        return _grid_size(self._bounds, self._resolution)
 
     def __call__(self, sample: Sample) -> np.ndarray:
         points = np.asarray(sample.data[self._points_key])

{apairo_preprocess-0.2.0 → apairo_preprocess-0.3.0}/apairo_preprocess/projection/image_mask.py

@@ -50,7 +50,9 @@ class ImageMaskFromPointLabels(FramePreprocessor):
         labels_key:      Input channel with per-point integer labels ``(N,)``,
                          row-aligned with the projection.
         projection_key:  Input channel with ``(N, 3)`` ``[u, v, depth]`` rows
-                         from :class:`LidarCameraProjection`.
+                         from :class:`LidarCameraProjection`.  A point paints
+                         pixel ``(floor(v), floor(u))``; rows that are NaN or
+                         whose pixel falls outside the image are not painted.
         radius:          Splat radius in pixels.  ``0`` (default) paints one
                          pixel per point; ``r`` paints a disc of radius ``r``,
                          densifying the sparse lidar coverage.
@@ -121,14 +123,31 @@ class ImageMaskFromPointLabels(FramePreprocessor):
         height, width = self._height, self._width
         mask = np.full((height, width), self._unknown, dtype=np.uint8)
 
-        valid = np.isfinite(uv[:, 0])
+        # Pixel index = floor of the stored coordinate, and a point is painted
+        # only if that index is inside the image: the bound test and the index
+        # come from the same values, so a point on (or past) the edge is
+        # dropped as a whole instead of splatting its in-image neighbours or
+        # aliasing into another occlusion bin.
+        with np.errstate(invalid="ignore"):
+            cols_f = np.floor(uv[:, 0])
+            rows_f = np.floor(uv[:, 1])
+            valid = (
+                np.isfinite(cols_f) & np.isfinite(rows_f)
+                & (cols_f >= 0) & (cols_f < width)
+                & (rows_f >= 0) & (rows_f < height)
+            )
         if not valid.any():
             return mask
 
-        cols = uv[valid, 0].astype(np.intp)
-        rows = uv[valid, 1].astype(np.intp)
+        cols = cols_f[valid].astype(np.intp)
+        rows = rows_f[valid].astype(np.intp)
         depth = uv[valid, 2].astype(np.float64)
         vals = labels[valid]
+        if vals.dtype.kind == "f" and not np.all(vals == np.floor(vals)):
+            raise ValueError(
+                "labels must be integers (class ids); got fractional values, "
+                "which a mask would truncate."
+            )
         if vals.min() < 0 or vals.max() > 255:
             raise ValueError(
                 f"labels must fit uint8, got range [{vals.min()}, {vals.max()}]"

{apairo_preprocess-0.2.0 → apairo_preprocess-0.3.0}/apairo_preprocess/projection/lidar_to_camera.py

@@ -52,6 +52,22 @@ def _distort_plumb_bob(x: np.ndarray, y: np.ndarray, d: np.ndarray) -> tuple:
     return xd, yd
 
 
+def _monotonic_limit(d: np.ndarray) -> float:
+    """The squared radius below which the radial model is one-to-one.
+
+    The distorted radius is ``r * (1 + k1 r^2 + k2 r^4 + k3 r^6)``. Past the
+    radius where its derivative first reaches zero the polynomial turns back:
+    a point far outside the field of view is folded onto the image, as a
+    valid-looking pixel. That limit, as ``r^2``; ``inf`` when the model never
+    turns back.
+    """
+    k1, k2, _, _, k3 = d
+    # d/dr [r (1 + k1 s + k2 s^2 + k3 s^3)] with s = r^2
+    roots = np.roots([7.0 * k3, 5.0 * k2, 3.0 * k1, 1.0])
+    positive = [float(r.real) for r in roots if abs(r.imag) < 1e-12 and r.real > 0]
+    return min(positive) if positive else float("inf")
+
+
 class LidarCameraProjection(FramePreprocessor):
     """Per-point pixel coordinates of a lidar scan in a camera image.
 
@@ -134,8 +150,10 @@ class LidarCameraProjection(FramePreprocessor):
                     f"distortion must be (k1, k2, p1, p2[, k3]), got {d.size} values"
                 )
             self._dist = d
+            self._r2_limit = _monotonic_limit(d)
         else:
             self._dist = None
+            self._r2_limit = float("inf")
 
         self._K = K
         self._T = T
@@ -158,6 +176,13 @@ class LidarCameraProjection(FramePreprocessor):
 
         out = np.full((n, 3), np.nan, dtype=np.float32)
         front = z > 1e-9
+        if self._dist is not None:
+            # Outside the radius where the distortion model is one-to-one, a
+            # point is not in the image, whatever pixel the polynomial gives:
+            # far off-axis lidar returns were folded back into the picture.
+            with np.errstate(invalid="ignore", divide="ignore"):
+                r2 = (cam[:, 0] ** 2 + cam[:, 1] ** 2) / (z * z)
+            front &= r2 < self._r2_limit
         if not front.any():
             return out
 
@@ -166,8 +191,11 @@ class LidarCameraProjection(FramePreprocessor):
         if self._dist is not None:
             x, y = _distort_plumb_bob(x, y, self._dist)
 
-        u = self._K[0, 0] * x + self._K[0, 1] * y + self._K[0, 2]
-        v = self._K[1, 1] * y + self._K[1, 2]
+        # The bound test runs on the float32 values actually returned: a u
+        # just below the width in float64 can round up to exactly the width
+        # in float32, and must then be outside, not one pixel past the edge.
+        u = (self._K[0, 0] * x + self._K[0, 1] * y + self._K[0, 2]).astype(np.float32)
+        v = (self._K[1, 1] * y + self._K[1, 2]).astype(np.float32)
 
         in_image = (u >= 0) & (u < self._width) & (v >= 0) & (v < self._height)
         idx = np.flatnonzero(front)[in_image]

{apairo_preprocess-0.2.0 → apairo_preprocess-0.3.0}/apairo_preprocess/projection/point_features.py

@@ -89,9 +89,13 @@ class PointFeaturesFromImage(FramePreprocessor):
         out = np.full((n, channels), self._fill_value, dtype=img.dtype)
 
         valid = np.isfinite(uv[:, 0])
-        cols = uv[valid, 0].astype(np.intp)
-        rows = uv[valid, 1].astype(np.intp)
-        if len(cols) and (cols.max() >= width or rows.max() >= height):
+        # floor, not astype: truncation would send u in (-1, 0) to column 0,
+        # and a negative index would silently wrap to the opposite edge.
+        cols = np.floor(uv[valid, 0]).astype(np.intp)
+        rows = np.floor(uv[valid, 1]).astype(np.intp)
+        if len(cols) and (
+            cols.min() < 0 or rows.min() < 0 or cols.max() >= width or rows.max() >= height
+        ):
             raise ValueError(
                 f"projection exceeds image bounds ({height}, {width}) — the "
                 f"projection was computed for a different image_size."

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

@@ -31,6 +31,20 @@ from apairo.core.preprocessor import SequencePreprocessor
 from apairo.core.sample import Sample
 from apairo_transform import RangeFilter
 
+class _OrderNorm:
+    """``(N, 3) -> (N,)`` norm of a given order (``2`` a sphere, ``np.inf`` a
+    cube): the callable :class:`~apairo_transform.RangeFilter` takes."""
+
+    def __init__(self, order: float) -> None:
+        self.order = float(order)
+
+    def __call__(self, points: np.ndarray) -> np.ndarray:
+        return np.linalg.norm(points, ord=self.order, axis=1)
+
+    def __repr__(self) -> str:
+        return f"_OrderNorm({self.order})"
+
+
 class TraversabilityFromTrajectory(SequencePreprocessor):
     """Label each point traversable if it lies in the robot's forward footprint.
 
@@ -44,10 +58,33 @@ class TraversabilityFromTrajectory(SequencePreprocessor):
                                float64.  Apply ``PoseTo4x4()`` from ``apairo_transform``
                                if needed.
         robot_radius:          Half-width of the robot footprint in XY (metres).
-        height_min:            Minimum point height relative to the nearest robot
-                               position to be traversable (metres, ≤ 0).
-        height_max:            Maximum point height relative to the nearest robot
-                               position (metres, ≥ 0).
+                               Isotropic circle test, ignored when ``robot_length`` /
+                               ``robot_width`` are given.
+        robot_length:          Full length of the robot footprint along its heading
+                               (metres). Must be given together with ``robot_width``.
+                               When set, the footprint is a heading-aligned rectangle
+                               (matching the physical robot, e.g. its wheel-contact
+                               box) instead of ``robot_radius``'s isotropic circle —
+                               use this whenever the robot is not roughly as wide as
+                               it is long, since a circle sized to the robot's length
+                               over-labels its width (and vice versa). Heading is read
+                               from each future pose's own rotation matrix, not
+                               finite-differenced from consecutive positions, so it
+                               stays accurate even where the robot is nearly
+                               stationary (position noise dominates finite differences
+                               there).
+        robot_width:           Full width of the robot footprint, lateral to its
+                               heading (metres). See ``robot_length``.
+        height_min:            Lowest point height, relative to the ground under a
+                               future pose, to be traversable (metres, ≤ 0).
+        height_max:            Highest point height relative to that ground
+                               (metres, ≥ 0).
+        pose_height:           Height of the pose frame's origin above the ground
+                               (metres). ``0`` (default) for poses given at
+                               ground level. With lidar odometry (KISS-ICP, GICP)
+                               the pose is the sensor's: pass the lidar's
+                               mounting height, or every ground point lies below
+                               the window and no point is ever traversable.
         forward_window:        Maximum number of future poses to look ahead.
                                ``None`` (default) uses the entire remaining trajectory.
         forward_distance:      Maximum *path length* (metres) of future trajectory
@@ -64,10 +101,10 @@ class TraversabilityFromTrajectory(SequencePreprocessor):
                                (e.g. humans) close to the robot.  The shape of the
                                exclusion zone is controlled by ``near_exclusion_norm``.
                                ``0.0`` disables the exclusion (default).
-        near_exclusion_norm:   Norm order for the exclusion distance, forwarded to
-                               :class:`~apairo_transform.RangeFilter`.  Use
-                               ``np.inf`` (L∞) for a cube, ``2`` for a sphere.
-                               Defaults to ``np.inf``.
+        near_exclusion_norm:   Norm order for the exclusion distance: ``np.inf``
+                               (L∞) for a cube, ``2`` for a sphere. Defaults to
+                               ``np.inf``. A callable ``(N, 3) -> (N,)`` is
+                               accepted too.
         output_key:            Override the default output channel name ``"trav_traj"``.
     """
 
@@ -81,6 +118,8 @@ class TraversabilityFromTrajectory(SequencePreprocessor):
         lidar_key: str = "lidar",
         poses_key: str = "poses",
         robot_radius: float = 0.75,
+        robot_length: float | None = None,
+        robot_width: float | None = None,
         height_min: float = -0.3,
         height_max: float = 0.5,
         forward_window: int | None = None,
@@ -89,17 +128,35 @@ class TraversabilityFromTrajectory(SequencePreprocessor):
         near_exclusion_radius: float = 0.0,
         near_exclusion_norm: float = np.inf,
         output_key: str | None = None,
+        pose_height: float = 0.0,
     ) -> None:
+        if (robot_length is None) != (robot_width is None):
+            raise ValueError("robot_length and robot_width must be given together")
+
         self._lidar_key = lidar_key
         self._poses_key = poses_key
         self._robot_radius = robot_radius
-        self._height_min = height_min
-        self._height_max = height_max
+        self._robot_length = robot_length
+        self._robot_width = robot_width
+        self._oriented = robot_length is not None
+        # Heights are tested against the pose origin: the window is shifted
+        # down by the origin's height above the ground.
+        self.pose_height = float(pose_height)
+        self._height_min = height_min - self.pose_height
+        self._height_max = height_max - self.pose_height
         self._forward_window = forward_window
         self._forward_distance = forward_distance
         self._sequence_gap = sequence_gap
         self._near_filter = (
-            RangeFilter(min=near_exclusion_radius, max=None, norm=near_exclusion_norm)
+            RangeFilter(
+                min=near_exclusion_radius,
+                max=None,
+                norm=(
+                    near_exclusion_norm
+                    if callable(near_exclusion_norm)
+                    else _OrderNorm(near_exclusion_norm)
+                ),
+            )
             if near_exclusion_radius > 0
             else None
         )
@@ -112,6 +169,8 @@ class TraversabilityFromTrajectory(SequencePreprocessor):
     def __call__(self, frames: Iterator[Sample]) -> np.ndarray:
         all_samples = list(frames)
         n = len(all_samples)
+        if n == 0:
+            return np.empty(0, dtype=object)
 
         poses = np.asarray(
             np.stack([s.data[self._poses_key] for s in all_samples]), dtype=np.float64
@@ -132,10 +191,22 @@ class TraversabilityFromTrajectory(SequencePreprocessor):
         # Cumulative path length, for the forward_distance cap.
         path = np.concatenate([[0.0], np.cumsum(dists)])
 
+        if self._oriented:
+            # Yaw from each pose's own rotation matrix -- robust where the robot is
+            # nearly stationary (finite-differenced heading is noise-dominated there).
+            headings = np.arctan2(poses[:, 1, 0], poses[:, 0, 0])
+            half_length = self._robot_length / 2.0
+            half_width = self._robot_width / 2.0
+            covering_radius = float(np.hypot(half_length, half_width))
+
         results = []
         for idx, sample in enumerate(all_samples):
             pc = np.asarray(sample.data[self._lidar_key])
-            xyz_sensor = pc[:, :3].astype(np.float64)
+            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.
+            finite = np.all(np.isfinite(xyz_all), axis=1)
+            xyz_sensor = xyz_all[finite]
             n_pts = len(xyz_sensor)
 
             T = poses[idx]
@@ -153,23 +224,49 @@ class TraversabilityFromTrajectory(SequencePreprocessor):
             future_pos = poses[idx + 1 : end, :3, 3]
 
             if len(future_pos) == 0:
-                results.append(np.zeros(n_pts, dtype=np.uint8))
+                results.append(np.zeros(len(xyz_all), dtype=np.uint8))
                 continue
 
+            # Every future pose whose footprint may hold the point is tested, in
+            # both modes: a path that passes over a point at its own height and
+            # crosses the same spot again higher up (a ramp, a bridge, vertical
+            # drift on a loop) still drove over it.
+            reach = covering_radius if self._oriented else float(self._robot_radius)
             tree = KDTree(future_pos[:, :2])
             # workers=-1 deadlocks when called from a ThreadPoolExecutor (e.g. viewer)
-            dist_xy, nn_idx = tree.query(xyz_world[:, :2], k=1)
-            dz = xyz_world[:, 2] - future_pos[nn_idx, 2]
-
-            trav = (
-                (dist_xy < self._robot_radius)
-                & (dz >= self._height_min)
-                & (dz <= self._height_max)
-            )
+            cand_lists = tree.query_ball_point(xyz_world[:, :2], r=reach)
+            lengths = np.fromiter((len(c) for c in cand_lists), dtype=np.int64, count=n_pts)
+            trav = np.zeros(n_pts, dtype=bool)
+            n_pairs = int(lengths.sum())
+            if n_pairs > 0:
+                point_idx = np.repeat(np.arange(n_pts), lengths)
+                cand_idx = np.fromiter(
+                    (c for cl in cand_lists for c in cl), dtype=np.int64, count=n_pairs
+                )
+                ddx = xyz_world[point_idx, 0] - future_pos[cand_idx, 0]
+                ddy = xyz_world[point_idx, 1] - future_pos[cand_idx, 1]
+                dz_pair = xyz_world[point_idx, 2] - future_pos[cand_idx, 2]
+                if self._oriented:
+                    th = headings[idx + 1 : end][cand_idx]
+                    c, s = np.cos(th), np.sin(th)
+                    # world delta -> candidate pose's local frame (rotate by -heading)
+                    dx = c * ddx + s * ddy
+                    dy = -s * ddx + c * ddy
+                    footprint = (np.abs(dx) <= half_length) & (np.abs(dy) <= half_width)
+                else:
+                    footprint = np.hypot(ddx, ddy) < self._robot_radius
+                inside = (
+                    footprint
+                    & (dz_pair >= self._height_min)
+                    & (dz_pair <= self._height_max)
+                )
+                np.logical_or.at(trav, point_idx[inside], True)
             if self._near_filter is not None:
                 trav &= self._near_filter.compute_mask(xyz_sensor)
 
-            results.append(trav.astype(np.uint8))
+            labels = np.zeros(len(xyz_all), dtype=np.uint8)
+            labels[finite] = trav
+            results.append(labels)
 
         # Uniform scans stack to (N, P) as before; ragged scans (real lidar,
         # variable return count) become an object array of per-frame rows,

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

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: apairo-preprocess
-Version: 0.2.0
+Version: 0.3.0
 Summary: Preprocessing for apairo datasets: odometry, ground segmentation, projection, traversability labels
 Author-email: Augustin Bresset <augustin.bresset@gmail.com>
 License: MIT
@@ -183,13 +183,19 @@ SemanticKittiDataset.run_preprocess(
 
 Requires poses to be computed first (e.g. with `KissICPOdometry`).
 
+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
 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)