apairo-preprocess 0.2.0__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- apairo_preprocess/__init__.py +27 -0
- apairo_preprocess/ground/__init__.py +9 -0
- apairo_preprocess/ground/csf.py +95 -0
- apairo_preprocess/ground/from_labels.py +55 -0
- apairo_preprocess/ground/ransac.py +95 -0
- apairo_preprocess/odometry/__init__.py +4 -0
- apairo_preprocess/odometry/gicp.py +96 -0
- apairo_preprocess/odometry/kissicp.py +84 -0
- apairo_preprocess/pointcloud/__init__.py +4 -0
- apairo_preprocess/pointcloud/robot_filter.py +121 -0
- apairo_preprocess/pointcloud/voxelise.py +281 -0
- apairo_preprocess/priors/__init__.py +7 -0
- apairo_preprocess/priors/ground_height.py +60 -0
- apairo_preprocess/priors/trajectory_distance.py +97 -0
- apairo_preprocess/projection/__init__.py +12 -0
- apairo_preprocess/projection/bev.py +166 -0
- apairo_preprocess/projection/image_mask.py +181 -0
- apairo_preprocess/projection/lidar_to_camera.py +177 -0
- apairo_preprocess/projection/point_features.py +100 -0
- apairo_preprocess/traversability/__init__.py +7 -0
- apairo_preprocess/traversability/from_labels.py +61 -0
- apairo_preprocess/traversability/from_trajectory.py +181 -0
- apairo_preprocess-0.2.0.dist-info/METADATA +255 -0
- apairo_preprocess-0.2.0.dist-info/RECORD +27 -0
- apairo_preprocess-0.2.0.dist-info/WHEEL +5 -0
- apairo_preprocess-0.2.0.dist-info/licenses/LICENSE +21 -0
- apairo_preprocess-0.2.0.dist-info/top_level.txt +1 -0
|
@@ -0,0 +1,281 @@
|
|
|
1
|
+
"""Voxel-grid downsampling of LiDAR point clouds and their semantic labels.
|
|
2
|
+
|
|
3
|
+
Three preprocessors share a common voxel assignment and must be run with the
|
|
4
|
+
same ``voxel_size`` and ``max_range`` to stay index-aligned:
|
|
5
|
+
|
|
6
|
+
* :class:`VoxelisePointCloud` — one representative float point per voxel.
|
|
7
|
+
* :class:`VoxeliseLabels` — one aggregated label per voxel (majority/max).
|
|
8
|
+
* :class:`VoxeliseCoords` — integer voxel coordinates (N', 3) int32,
|
|
9
|
+
ready for sparse convolution networks.
|
|
10
|
+
|
|
11
|
+
After preprocessing, ``voxelised[i]``, ``voxelised_labels[i]``, and
|
|
12
|
+
``voxel_coords[i]`` all refer to the same voxel cell.
|
|
13
|
+
|
|
14
|
+
Typical usage::
|
|
15
|
+
|
|
16
|
+
for prep in [
|
|
17
|
+
VoxelisePointCloud(voxel_size=0.1, max_range=50.0),
|
|
18
|
+
VoxeliseLabels(voxel_size=0.1, max_range=50.0),
|
|
19
|
+
VoxeliseCoords(voxel_size=0.1, max_range=50.0),
|
|
20
|
+
]:
|
|
21
|
+
dataset.run_preprocess(prep, split_dir)
|
|
22
|
+
"""
|
|
23
|
+
|
|
24
|
+
from __future__ import annotations
|
|
25
|
+
|
|
26
|
+
from typing import ClassVar, Literal
|
|
27
|
+
|
|
28
|
+
import numpy as np
|
|
29
|
+
|
|
30
|
+
from apairo.core.preprocessor import FramePreprocessor
|
|
31
|
+
from apairo.core.sample import Sample
|
|
32
|
+
|
|
33
|
+
_Reduction = Literal["centroid", "first", "random"]
|
|
34
|
+
_LabelAggregation = Literal["majority", "max"]
|
|
35
|
+
|
|
36
|
+
|
|
37
|
+
def _filter_and_quantize(
|
|
38
|
+
xyz: np.ndarray,
|
|
39
|
+
voxel_size: float,
|
|
40
|
+
max_range: float | None,
|
|
41
|
+
) -> tuple[np.ndarray | None, np.ndarray, np.ndarray, int]:
|
|
42
|
+
"""Apply range filter and compute voxel assignment.
|
|
43
|
+
|
|
44
|
+
Returns:
|
|
45
|
+
mask: Boolean keep-mask (or None if no filtering was done).
|
|
46
|
+
unique_coords: Integer voxel coordinates, shape (n_voxels, 3) int32.
|
|
47
|
+
inverse: Per-point voxel index array, shape (N_kept,).
|
|
48
|
+
n_voxels: Number of occupied voxels.
|
|
49
|
+
"""
|
|
50
|
+
mask: np.ndarray | None = None
|
|
51
|
+
if max_range is not None:
|
|
52
|
+
mask = np.linalg.norm(xyz, axis=1) < max_range
|
|
53
|
+
xyz = xyz[mask]
|
|
54
|
+
|
|
55
|
+
if len(xyz) == 0:
|
|
56
|
+
empty_coords = np.empty((0, 3), dtype=np.int32)
|
|
57
|
+
return mask, empty_coords, np.empty(0, dtype=np.int64), 0
|
|
58
|
+
|
|
59
|
+
coords = np.floor(xyz / voxel_size).astype(np.int32)
|
|
60
|
+
unique_coords, inverse = np.unique(coords, axis=0, return_inverse=True)
|
|
61
|
+
return mask, unique_coords, inverse, len(unique_coords)
|
|
62
|
+
|
|
63
|
+
|
|
64
|
+
class VoxelisePointCloud(FramePreprocessor):
|
|
65
|
+
"""Downsample a point cloud with a voxel grid.
|
|
66
|
+
|
|
67
|
+
Each voxel retains exactly one point according to ``reduction``:
|
|
68
|
+
|
|
69
|
+
* ``"centroid"`` — mean of all points falling in the voxel (all channels).
|
|
70
|
+
* ``"first"`` — first point encountered (input order).
|
|
71
|
+
* ``"random"`` — one uniformly random point per voxel.
|
|
72
|
+
|
|
73
|
+
Args:
|
|
74
|
+
lidar_key: Input channel for point cloud data.
|
|
75
|
+
voxel_size: Edge length of each cubic voxel cell (metres).
|
|
76
|
+
max_range: Discard points farther than this distance from the sensor
|
|
77
|
+
origin (metres). ``None`` keeps all points.
|
|
78
|
+
reduction: How to pick the representative point per voxel.
|
|
79
|
+
output_key: Override the default output channel name ``"voxelised"``.
|
|
80
|
+
"""
|
|
81
|
+
|
|
82
|
+
output_key: ClassVar[str] = "voxelised"
|
|
83
|
+
input_keys: ClassVar[list[str]] = ["lidar"]
|
|
84
|
+
timestamps_from: ClassVar[str] = "lidar"
|
|
85
|
+
sources: ClassVar[list[str]] = ["lidar"]
|
|
86
|
+
|
|
87
|
+
def __init__(
|
|
88
|
+
self,
|
|
89
|
+
lidar_key: str = "lidar",
|
|
90
|
+
voxel_size: float = 0.1,
|
|
91
|
+
max_range: float | None = None,
|
|
92
|
+
reduction: _Reduction = "centroid",
|
|
93
|
+
output_key: str | None = None,
|
|
94
|
+
) -> None:
|
|
95
|
+
if voxel_size <= 0:
|
|
96
|
+
raise ValueError(f"voxel_size must be positive, got {voxel_size}")
|
|
97
|
+
if max_range is not None and max_range <= 0:
|
|
98
|
+
raise ValueError(f"max_range must be positive, got {max_range}")
|
|
99
|
+
if reduction not in ("centroid", "first", "random"):
|
|
100
|
+
raise ValueError(
|
|
101
|
+
f"reduction must be 'centroid', 'first', or 'random', got {reduction!r}"
|
|
102
|
+
)
|
|
103
|
+
|
|
104
|
+
self._lidar_key = lidar_key
|
|
105
|
+
self._voxel_size = float(voxel_size)
|
|
106
|
+
self._max_range = max_range
|
|
107
|
+
self._reduction = reduction
|
|
108
|
+
|
|
109
|
+
self.input_keys = [lidar_key]
|
|
110
|
+
self.sources = [lidar_key]
|
|
111
|
+
if output_key is not None:
|
|
112
|
+
self.output_key = output_key
|
|
113
|
+
|
|
114
|
+
def __call__(self, sample: Sample) -> np.ndarray:
|
|
115
|
+
pc = np.asarray(sample.data[self._lidar_key], dtype=np.float64)
|
|
116
|
+
if pc.ndim != 2 or pc.shape[1] < 3:
|
|
117
|
+
raise ValueError(f"Point cloud must be (N, D>=3), got {pc.shape}")
|
|
118
|
+
|
|
119
|
+
mask, _, inverse, n_voxels = _filter_and_quantize(
|
|
120
|
+
pc[:, :3], self._voxel_size, self._max_range
|
|
121
|
+
)
|
|
122
|
+
if mask is not None:
|
|
123
|
+
pc = pc[mask]
|
|
124
|
+
|
|
125
|
+
if n_voxels == 0:
|
|
126
|
+
return np.empty((0, pc.shape[1]), dtype=np.float64)
|
|
127
|
+
|
|
128
|
+
if self._reduction == "centroid":
|
|
129
|
+
out = np.zeros((n_voxels, pc.shape[1]), dtype=np.float64)
|
|
130
|
+
counts = np.zeros(n_voxels, dtype=np.int64)
|
|
131
|
+
np.add.at(out, inverse, pc)
|
|
132
|
+
np.add.at(counts, inverse, 1)
|
|
133
|
+
out /= counts[:, None]
|
|
134
|
+
return out
|
|
135
|
+
|
|
136
|
+
if self._reduction == "first":
|
|
137
|
+
out = np.empty((n_voxels, pc.shape[1]), dtype=np.float64)
|
|
138
|
+
# assign in reverse so the first occurrence wins
|
|
139
|
+
out[inverse] = pc
|
|
140
|
+
return out
|
|
141
|
+
|
|
142
|
+
# random: shuffle so a random point ends up last (and thus selected)
|
|
143
|
+
rng = np.random.default_rng()
|
|
144
|
+
chosen = np.empty(n_voxels, dtype=np.int64)
|
|
145
|
+
perm = rng.permutation(len(pc))
|
|
146
|
+
chosen[inverse[perm]] = perm
|
|
147
|
+
return pc[chosen]
|
|
148
|
+
|
|
149
|
+
|
|
150
|
+
class VoxeliseLabels(FramePreprocessor):
|
|
151
|
+
"""Aggregate per-point semantic labels onto the same voxel grid.
|
|
152
|
+
|
|
153
|
+
Must be run with the **same** ``voxel_size`` and ``max_range`` as
|
|
154
|
+
:class:`VoxelisePointCloud` so the output channels remain index-aligned.
|
|
155
|
+
|
|
156
|
+
* ``"majority"`` — most frequent label per voxel (semantically correct).
|
|
157
|
+
* ``"max"`` — highest label ID per voxel (fast, mirrors common
|
|
158
|
+
sparse-convolution data loaders).
|
|
159
|
+
|
|
160
|
+
Args:
|
|
161
|
+
lidar_key: Input channel for point cloud data (geometry only).
|
|
162
|
+
labels_key: Input channel for per-point semantic labels.
|
|
163
|
+
voxel_size: Edge length of each cubic voxel cell (metres).
|
|
164
|
+
max_range: Same range filter as applied to the point cloud.
|
|
165
|
+
aggregation: Label merging strategy per voxel.
|
|
166
|
+
output_key: Override the default output channel name
|
|
167
|
+
``"voxelised_labels"``.
|
|
168
|
+
"""
|
|
169
|
+
|
|
170
|
+
output_key: ClassVar[str] = "voxelised_labels"
|
|
171
|
+
input_keys: ClassVar[list[str]] = ["lidar", "labels"]
|
|
172
|
+
timestamps_from: ClassVar[str] = "lidar"
|
|
173
|
+
sources: ClassVar[list[str]] = ["lidar", "labels"]
|
|
174
|
+
|
|
175
|
+
def __init__(
|
|
176
|
+
self,
|
|
177
|
+
lidar_key: str = "lidar",
|
|
178
|
+
labels_key: str = "labels",
|
|
179
|
+
voxel_size: float = 0.1,
|
|
180
|
+
max_range: float | None = None,
|
|
181
|
+
aggregation: _LabelAggregation = "majority",
|
|
182
|
+
output_key: str | None = None,
|
|
183
|
+
) -> None:
|
|
184
|
+
if voxel_size <= 0:
|
|
185
|
+
raise ValueError(f"voxel_size must be positive, got {voxel_size}")
|
|
186
|
+
if max_range is not None and max_range <= 0:
|
|
187
|
+
raise ValueError(f"max_range must be positive, got {max_range}")
|
|
188
|
+
if aggregation not in ("majority", "max"):
|
|
189
|
+
raise ValueError(f"aggregation must be 'majority' or 'max', got {aggregation!r}")
|
|
190
|
+
|
|
191
|
+
self._lidar_key = lidar_key
|
|
192
|
+
self._labels_key = labels_key
|
|
193
|
+
self._voxel_size = float(voxel_size)
|
|
194
|
+
self._max_range = max_range
|
|
195
|
+
self._aggregation = aggregation
|
|
196
|
+
|
|
197
|
+
self.input_keys = [lidar_key, labels_key]
|
|
198
|
+
self.sources = [lidar_key, labels_key]
|
|
199
|
+
if output_key is not None:
|
|
200
|
+
self.output_key = output_key
|
|
201
|
+
|
|
202
|
+
def __call__(self, sample: Sample) -> np.ndarray:
|
|
203
|
+
xyz = np.asarray(sample.data[self._lidar_key], dtype=np.float64)[:, :3]
|
|
204
|
+
labels = np.asarray(sample.data[self._labels_key])
|
|
205
|
+
|
|
206
|
+
mask, _, inverse, n_voxels = _filter_and_quantize(
|
|
207
|
+
xyz, self._voxel_size, self._max_range
|
|
208
|
+
)
|
|
209
|
+
if mask is not None:
|
|
210
|
+
labels = labels[mask]
|
|
211
|
+
|
|
212
|
+
if n_voxels == 0:
|
|
213
|
+
return np.empty(0, dtype=labels.dtype)
|
|
214
|
+
|
|
215
|
+
if self._aggregation == "max":
|
|
216
|
+
out = np.zeros(n_voxels, dtype=labels.dtype)
|
|
217
|
+
np.maximum.at(out, inverse, labels)
|
|
218
|
+
return out
|
|
219
|
+
|
|
220
|
+
# majority: encode (voxel_idx, label) as a single int and bincount
|
|
221
|
+
n_classes = int(labels.max()) + 1
|
|
222
|
+
keys = inverse.astype(np.int64) * n_classes + labels.astype(np.int64)
|
|
223
|
+
pair_counts = np.bincount(keys, minlength=n_voxels * n_classes)
|
|
224
|
+
return pair_counts.reshape(n_voxels, n_classes).argmax(axis=1).astype(labels.dtype)
|
|
225
|
+
|
|
226
|
+
|
|
227
|
+
class VoxeliseCoords(FramePreprocessor):
|
|
228
|
+
"""Save integer voxel coordinates for each occupied cell.
|
|
229
|
+
|
|
230
|
+
Produces an ``(N', 3)`` int32 array where row ``i`` is the quantised
|
|
231
|
+
coordinate ``floor(xyz / voxel_size)`` of the ``i``-th voxel. This is the
|
|
232
|
+
format expected by sparse convolution frameworks (torchsparse,
|
|
233
|
+
MinkowskiEngine) and eliminates the ``floor`` recomputation from every
|
|
234
|
+
``__getitem__``.
|
|
235
|
+
|
|
236
|
+
Must be run with the **same** ``voxel_size`` and ``max_range`` as
|
|
237
|
+
:class:`VoxelisePointCloud` and :class:`VoxeliseLabels` so all three
|
|
238
|
+
channels stay index-aligned.
|
|
239
|
+
|
|
240
|
+
Args:
|
|
241
|
+
lidar_key: Input channel for point cloud data.
|
|
242
|
+
voxel_size: Edge length of each cubic voxel cell (metres).
|
|
243
|
+
max_range: Same range filter as applied to the point cloud.
|
|
244
|
+
output_key: Override the default output channel name ``"voxel_coords"``.
|
|
245
|
+
"""
|
|
246
|
+
|
|
247
|
+
output_key: ClassVar[str] = "voxel_coords"
|
|
248
|
+
input_keys: ClassVar[list[str]] = ["lidar"]
|
|
249
|
+
timestamps_from: ClassVar[str] = "lidar"
|
|
250
|
+
sources: ClassVar[list[str]] = ["lidar"]
|
|
251
|
+
|
|
252
|
+
def __init__(
|
|
253
|
+
self,
|
|
254
|
+
lidar_key: str = "lidar",
|
|
255
|
+
voxel_size: float = 0.1,
|
|
256
|
+
max_range: float | None = None,
|
|
257
|
+
output_key: str | None = None,
|
|
258
|
+
) -> None:
|
|
259
|
+
if voxel_size <= 0:
|
|
260
|
+
raise ValueError(f"voxel_size must be positive, got {voxel_size}")
|
|
261
|
+
if max_range is not None and max_range <= 0:
|
|
262
|
+
raise ValueError(f"max_range must be positive, got {max_range}")
|
|
263
|
+
|
|
264
|
+
self._lidar_key = lidar_key
|
|
265
|
+
self._voxel_size = float(voxel_size)
|
|
266
|
+
self._max_range = max_range
|
|
267
|
+
|
|
268
|
+
self.input_keys = [lidar_key]
|
|
269
|
+
self.sources = [lidar_key]
|
|
270
|
+
if output_key is not None:
|
|
271
|
+
self.output_key = output_key
|
|
272
|
+
|
|
273
|
+
def __call__(self, sample: Sample) -> np.ndarray:
|
|
274
|
+
xyz = np.asarray(sample.data[self._lidar_key], dtype=np.float64)[:, :3]
|
|
275
|
+
if xyz.ndim != 2 or xyz.shape[1] != 3:
|
|
276
|
+
raise ValueError(f"LiDAR channel must be (N, D>=3), got {xyz.shape}")
|
|
277
|
+
|
|
278
|
+
_, unique_coords, _, _ = _filter_and_quantize(
|
|
279
|
+
xyz, self._voxel_size, self._max_range
|
|
280
|
+
)
|
|
281
|
+
return unique_coords # (N', 3) int32
|
|
@@ -0,0 +1,60 @@
|
|
|
1
|
+
"""Height above ground surface from binary ground segmentation labels."""
|
|
2
|
+
|
|
3
|
+
from __future__ import annotations
|
|
4
|
+
|
|
5
|
+
from typing import ClassVar
|
|
6
|
+
|
|
7
|
+
import numpy as np
|
|
8
|
+
from scipy.spatial import KDTree
|
|
9
|
+
|
|
10
|
+
from apairo.core.preprocessor import FramePreprocessor
|
|
11
|
+
from apairo.core.sample import Sample
|
|
12
|
+
|
|
13
|
+
|
|
14
|
+
class GroundHeightFromLabels(FramePreprocessor):
|
|
15
|
+
"""Per-voxel height above ground from any binary ground segmentation.
|
|
16
|
+
|
|
17
|
+
Takes the output of any ground segmentation preprocessor
|
|
18
|
+
(``GroundSegmentationCSF``, ``GroundSegmentationRANSAC``,
|
|
19
|
+
``GroundSegmentationFromLabels``) and computes the signed height (m) of
|
|
20
|
+
each voxel above the nearest ground point in XY.
|
|
21
|
+
|
|
22
|
+
Args:
|
|
23
|
+
ground_key: Input channel for binary ground labels (0=ground,
|
|
24
|
+
1=non-ground). Typically ``"ground_csf"``,
|
|
25
|
+
``"ground_ransac"``, or ``"ground_labels"``.
|
|
26
|
+
voxelised_key: Input channel for the voxelised point cloud
|
|
27
|
+
(shape N x D≥3).
|
|
28
|
+
output_key: Override the default channel name ``"ground_height"``.
|
|
29
|
+
"""
|
|
30
|
+
|
|
31
|
+
output_key: ClassVar[str] = "ground_height"
|
|
32
|
+
input_keys: ClassVar[list[str]] = ["voxelised", "ground_labels"]
|
|
33
|
+
timestamps_from: ClassVar[str] = "voxelised"
|
|
34
|
+
sources: ClassVar[list[str]] = ["voxelised", "ground_labels"]
|
|
35
|
+
|
|
36
|
+
def __init__(
|
|
37
|
+
self,
|
|
38
|
+
ground_key: str,
|
|
39
|
+
voxelised_key: str = "voxelised",
|
|
40
|
+
output_key: str | None = None,
|
|
41
|
+
) -> None:
|
|
42
|
+
self._ground_key = ground_key
|
|
43
|
+
self._voxelised_key = voxelised_key
|
|
44
|
+
self.input_keys = [voxelised_key, ground_key]
|
|
45
|
+
self.sources = [voxelised_key, ground_key]
|
|
46
|
+
if output_key is not None:
|
|
47
|
+
self.output_key = output_key
|
|
48
|
+
|
|
49
|
+
def __call__(self, sample: Sample) -> np.ndarray:
|
|
50
|
+
xyz = np.asarray(sample.data[self._voxelised_key], dtype=np.float64)[:, :3]
|
|
51
|
+
labels = np.asarray(sample.data[self._ground_key])
|
|
52
|
+
ground_pts = xyz[labels == 0]
|
|
53
|
+
|
|
54
|
+
if len(ground_pts) == 0:
|
|
55
|
+
z_ground = float(np.percentile(xyz[:, 2], 5))
|
|
56
|
+
return (xyz[:, 2] - z_ground).astype(np.float32)
|
|
57
|
+
|
|
58
|
+
tree = KDTree(ground_pts[:, :2])
|
|
59
|
+
_, nn = tree.query(xyz[:, :2])
|
|
60
|
+
return (xyz[:, 2] - ground_pts[nn, 2]).astype(np.float32)
|
|
@@ -0,0 +1,97 @@
|
|
|
1
|
+
"""Trajectory-distance preprocessor for Weakly Informed PUNCE.
|
|
2
|
+
|
|
3
|
+
Computes the Euclidean distance (metres) from each voxelised point to the
|
|
4
|
+
nearest waypoint on the robot's recorded trajectory. Points that lie close
|
|
5
|
+
to the path the robot actually drove are likely traversable.
|
|
6
|
+
|
|
7
|
+
The output float32 array of shape (N,) is consumed by
|
|
8
|
+
:class:`WeaklyInformedPUNCELoss` to compute the Gaussian prior
|
|
9
|
+
|
|
10
|
+
π(p) = exp(−d(p, traj)² / (2σ²))
|
|
11
|
+
|
|
12
|
+
All trajectory waypoints must be passed at construction time
|
|
13
|
+
(as world-frame positions) so the preprocessor can build a single KD-tree
|
|
14
|
+
shared across frames.
|
|
15
|
+
|
|
16
|
+
Typical usage in a preprocess script::
|
|
17
|
+
|
|
18
|
+
ds = Rellis3DDataset(root, keys=["poses"]).transform("poses", PoseTo4x4())
|
|
19
|
+
all_poses = np.stack([ds[i].data["poses"] for i in range(len(ds))])
|
|
20
|
+
trajectory = all_poses[:, :3, 3] # (N_frames, 3) world positions
|
|
21
|
+
|
|
22
|
+
Rellis3DDataset.run_preprocess(
|
|
23
|
+
TrajectoryDistance(trajectory=trajectory),
|
|
24
|
+
root,
|
|
25
|
+
)
|
|
26
|
+
"""
|
|
27
|
+
|
|
28
|
+
from __future__ import annotations
|
|
29
|
+
|
|
30
|
+
from typing import ClassVar
|
|
31
|
+
|
|
32
|
+
import numpy as np
|
|
33
|
+
from scipy.spatial import KDTree
|
|
34
|
+
|
|
35
|
+
from apairo.core.preprocessor import FramePreprocessor
|
|
36
|
+
from apairo.core.sample import Sample
|
|
37
|
+
|
|
38
|
+
|
|
39
|
+
|
|
40
|
+
class TrajectoryDistance(FramePreprocessor):
|
|
41
|
+
"""Per-voxel Euclidean distance to the nearest trajectory waypoint.
|
|
42
|
+
|
|
43
|
+
For each frame, voxelised points are transformed to the world frame using
|
|
44
|
+
the current pose, then queried against a KD-tree built from all trajectory
|
|
45
|
+
waypoints.
|
|
46
|
+
|
|
47
|
+
Args:
|
|
48
|
+
trajectory: (M, 3) array of robot positions in world frame.
|
|
49
|
+
Typically ``all_poses[:, :3, 3]`` from the sequence.
|
|
50
|
+
voxelised_key: Input channel for the voxelised point cloud.
|
|
51
|
+
poses_key: Input channel for the current-frame pose — must be ``(4, 4)``
|
|
52
|
+
float64. Apply ``PoseTo4x4()`` from ``apairo_transform`` if needed.
|
|
53
|
+
output_key: Override the default channel name
|
|
54
|
+
``"trajectory_distance"``.
|
|
55
|
+
"""
|
|
56
|
+
|
|
57
|
+
output_key: ClassVar[str] = "trajectory_distance"
|
|
58
|
+
input_keys: ClassVar[list[str]] = ["voxelised", "poses"]
|
|
59
|
+
timestamps_from: ClassVar[str] = "voxelised"
|
|
60
|
+
sources: ClassVar[list[str]] = ["voxelised", "poses"]
|
|
61
|
+
|
|
62
|
+
def __init__(
|
|
63
|
+
self,
|
|
64
|
+
trajectory: np.ndarray,
|
|
65
|
+
voxelised_key: str = "voxelised",
|
|
66
|
+
poses_key: str = "poses",
|
|
67
|
+
output_key: str | None = None,
|
|
68
|
+
) -> None:
|
|
69
|
+
traj = np.asarray(trajectory, dtype=np.float64)
|
|
70
|
+
if traj.ndim != 2 or traj.shape[1] != 3:
|
|
71
|
+
raise ValueError(
|
|
72
|
+
f"trajectory must be (M, 3), got {traj.shape}"
|
|
73
|
+
)
|
|
74
|
+
self._tree = KDTree(traj)
|
|
75
|
+
self._voxelised_key = voxelised_key
|
|
76
|
+
self._poses_key = poses_key
|
|
77
|
+
self.input_keys = [voxelised_key, poses_key]
|
|
78
|
+
self.sources = [voxelised_key, poses_key]
|
|
79
|
+
if output_key is not None:
|
|
80
|
+
self.output_key = output_key
|
|
81
|
+
|
|
82
|
+
def __call__(self, sample: Sample) -> np.ndarray:
|
|
83
|
+
pc = np.asarray(sample.data[self._voxelised_key], dtype=np.float64)
|
|
84
|
+
pose = np.asarray(sample.data[self._poses_key], dtype=np.float64)
|
|
85
|
+
if pose.shape != (4, 4):
|
|
86
|
+
raise ValueError(
|
|
87
|
+
f"pose must be (4, 4) — apply PoseTo4x4() from apairo_transform first. "
|
|
88
|
+
f"Got {pose.shape}"
|
|
89
|
+
)
|
|
90
|
+
|
|
91
|
+
n = len(pc)
|
|
92
|
+
xyz_sensor = pc[:, :3]
|
|
93
|
+
xyz_h = np.column_stack([xyz_sensor, np.ones(n)])
|
|
94
|
+
xyz_world = (pose @ xyz_h.T).T[:, :3]
|
|
95
|
+
|
|
96
|
+
dist, _ = self._tree.query(xyz_world)
|
|
97
|
+
return dist.astype(np.float32)
|
|
@@ -0,0 +1,12 @@
|
|
|
1
|
+
from apairo_preprocess.projection.bev import BEVRasterisation, to_uint8_image
|
|
2
|
+
from apairo_preprocess.projection.image_mask import ImageMaskFromPointLabels
|
|
3
|
+
from apairo_preprocess.projection.lidar_to_camera import LidarCameraProjection
|
|
4
|
+
from apairo_preprocess.projection.point_features import PointFeaturesFromImage
|
|
5
|
+
|
|
6
|
+
__all__ = [
|
|
7
|
+
"BEVRasterisation",
|
|
8
|
+
"to_uint8_image",
|
|
9
|
+
"ImageMaskFromPointLabels",
|
|
10
|
+
"LidarCameraProjection",
|
|
11
|
+
"PointFeaturesFromImage",
|
|
12
|
+
]
|
|
@@ -0,0 +1,166 @@
|
|
|
1
|
+
"""Rasterise per-point values into a top-down (bird's-eye-view) grid.
|
|
2
|
+
|
|
3
|
+
Unlike :class:`LidarCameraProjection` + :class:`ImageMaskFromPointLabels`
|
|
4
|
+
(perspective projection into a camera, nearest-depth-wins occlusion), a BEV
|
|
5
|
+
grid needs no camera at all: points are binned by their XY position onto an
|
|
6
|
+
orthographic top-down grid, so it works on any lidar scan regardless of
|
|
7
|
+
whether a calibrated camera exists for it -- useful for datasets that ship
|
|
8
|
+
point clouds without a registered camera/lidar extrinsic.
|
|
9
|
+
|
|
10
|
+
Row 0 of the output is north (``ymax``), so plotting the grid directly reads
|
|
11
|
+
top-down.
|
|
12
|
+
|
|
13
|
+
Typical usage::
|
|
14
|
+
|
|
15
|
+
TartanKittiDataset.run_preprocess(
|
|
16
|
+
BEVRasterisation(bounds=(-20, 20, -20, 20), resolution=0.2,
|
|
17
|
+
points_key="lidar", values_key="trav_traj"),
|
|
18
|
+
seq_dir,
|
|
19
|
+
)
|
|
20
|
+
"""
|
|
21
|
+
|
|
22
|
+
from __future__ import annotations
|
|
23
|
+
|
|
24
|
+
from typing import ClassVar
|
|
25
|
+
|
|
26
|
+
import numpy as np
|
|
27
|
+
|
|
28
|
+
from apairo.core.preprocessor import FramePreprocessor
|
|
29
|
+
from apairo.core.sample import Sample
|
|
30
|
+
|
|
31
|
+
|
|
32
|
+
def _rasterise(points, values, *, bounds, resolution, reduce, background):
|
|
33
|
+
xmin, xmax, ymin, ymax = bounds
|
|
34
|
+
w = int(round((xmax - xmin) / resolution))
|
|
35
|
+
h = int(round((ymax - ymin) / resolution))
|
|
36
|
+
|
|
37
|
+
xy = np.asarray(points)[:, :2]
|
|
38
|
+
vals = np.asarray(values, dtype=np.float64)
|
|
39
|
+
|
|
40
|
+
col = np.floor((xy[:, 0] - xmin) / resolution).astype(np.int64)
|
|
41
|
+
row = np.floor((ymax - xy[:, 1]) / resolution).astype(np.int64)
|
|
42
|
+
inside = (col >= 0) & (col < w) & (row >= 0) & (row < h)
|
|
43
|
+
cell = row[inside] * w + col[inside]
|
|
44
|
+
vals = vals[inside]
|
|
45
|
+
|
|
46
|
+
grid = np.full(h * w, background, dtype=np.float64)
|
|
47
|
+
if reduce == "max":
|
|
48
|
+
# A plain np.maximum.at into `grid` would use `background` as an
|
|
49
|
+
# implicit floor -- wrong whenever real values can be below it (e.g.
|
|
50
|
+
# negative heights). Accumulate from -inf, then fill only the cells
|
|
51
|
+
# no point touched.
|
|
52
|
+
acc = np.full(h * w, -np.inf)
|
|
53
|
+
np.maximum.at(acc, cell, vals)
|
|
54
|
+
hit = np.isfinite(acc)
|
|
55
|
+
grid[hit] = acc[hit]
|
|
56
|
+
elif reduce == "last":
|
|
57
|
+
grid[cell] = vals
|
|
58
|
+
else: # "mean"
|
|
59
|
+
count = np.zeros(h * w)
|
|
60
|
+
total = np.zeros(h * w)
|
|
61
|
+
np.add.at(count, cell, 1.0)
|
|
62
|
+
np.add.at(total, cell, vals)
|
|
63
|
+
hit = count > 0
|
|
64
|
+
grid[hit] = total[hit] / count[hit]
|
|
65
|
+
return grid.reshape(h, w)
|
|
66
|
+
|
|
67
|
+
|
|
68
|
+
class BEVRasterisation(FramePreprocessor):
|
|
69
|
+
"""Per-point values splatted onto a top-down ``(H, W)`` grid.
|
|
70
|
+
|
|
71
|
+
Output is float64, row-major with row 0 at ``ymax`` (north up) and
|
|
72
|
+
column 0 at ``xmin``. Cells no point falls into hold ``background``.
|
|
73
|
+
|
|
74
|
+
Args:
|
|
75
|
+
bounds: ``(xmin, xmax, ymin, ymax)`` in metres, in the point
|
|
76
|
+
cloud's own frame (typically sensor- or robot-centred).
|
|
77
|
+
resolution: Metres per pixel.
|
|
78
|
+
points_key: Input channel for the point cloud, ``(N, >=2)``; only
|
|
79
|
+
the X/Y columns are used.
|
|
80
|
+
values_key: Input channel for the per-point scalar to rasterise
|
|
81
|
+
(e.g. height, a traversability label), ``(N,)``,
|
|
82
|
+
row-aligned with ``points_key``.
|
|
83
|
+
reduce: How to combine points landing in the same cell --
|
|
84
|
+
``"max"`` (default), ``"mean"`` or ``"last"``.
|
|
85
|
+
background: Value for cells no point falls into.
|
|
86
|
+
output_key: Override the default output channel name ``"bev"``.
|
|
87
|
+
"""
|
|
88
|
+
|
|
89
|
+
output_key: ClassVar[str] = "bev"
|
|
90
|
+
input_keys: ClassVar[list[str]] = ["lidar", "values"]
|
|
91
|
+
timestamps_from: ClassVar[str] = "lidar"
|
|
92
|
+
sources: ClassVar[list[str]] = ["lidar", "values"]
|
|
93
|
+
|
|
94
|
+
def __init__(
|
|
95
|
+
self,
|
|
96
|
+
bounds: tuple[float, float, float, float],
|
|
97
|
+
resolution: float,
|
|
98
|
+
points_key: str = "lidar",
|
|
99
|
+
values_key: str = "values",
|
|
100
|
+
reduce: str = "max",
|
|
101
|
+
background: float = 0.0,
|
|
102
|
+
output_key: str | None = None,
|
|
103
|
+
) -> None:
|
|
104
|
+
xmin, xmax, ymin, ymax = bounds
|
|
105
|
+
if not (xmax > xmin and ymax > ymin):
|
|
106
|
+
raise ValueError(f"bounds must have xmax > xmin and ymax > ymin, got {bounds}")
|
|
107
|
+
if resolution <= 0:
|
|
108
|
+
raise ValueError(f"resolution must be > 0, got {resolution}")
|
|
109
|
+
if reduce not in ("max", "mean", "last"):
|
|
110
|
+
raise ValueError(f"reduce must be 'max', 'mean' or 'last', got {reduce!r}")
|
|
111
|
+
|
|
112
|
+
self._bounds = (float(xmin), float(xmax), float(ymin), float(ymax))
|
|
113
|
+
self._resolution = float(resolution)
|
|
114
|
+
self._reduce = reduce
|
|
115
|
+
self._background = float(background)
|
|
116
|
+
self._points_key = points_key
|
|
117
|
+
self._values_key = values_key
|
|
118
|
+
|
|
119
|
+
self.input_keys = [points_key, values_key]
|
|
120
|
+
self.sources = [points_key, values_key]
|
|
121
|
+
self.timestamps_from = points_key
|
|
122
|
+
if output_key is not None:
|
|
123
|
+
self.output_key = output_key
|
|
124
|
+
|
|
125
|
+
@property
|
|
126
|
+
def shape(self) -> tuple[int, int]:
|
|
127
|
+
"""``(height, width)`` of the output grid, from ``bounds``/``resolution``."""
|
|
128
|
+
xmin, xmax, ymin, ymax = self._bounds
|
|
129
|
+
w = int(round((xmax - xmin) / self._resolution))
|
|
130
|
+
h = int(round((ymax - ymin) / self._resolution))
|
|
131
|
+
return h, w
|
|
132
|
+
|
|
133
|
+
def __call__(self, sample: Sample) -> np.ndarray:
|
|
134
|
+
points = np.asarray(sample.data[self._points_key])
|
|
135
|
+
values = np.asarray(sample.data[self._values_key])
|
|
136
|
+
if points.ndim != 2 or points.shape[1] < 2:
|
|
137
|
+
raise ValueError(f"points must be (N, >=2), got {points.shape}")
|
|
138
|
+
if len(values) != len(points):
|
|
139
|
+
raise ValueError(
|
|
140
|
+
f"points ({len(points)}) and values ({len(values)}) are not "
|
|
141
|
+
f"row-aligned -- they must derive from the same scan."
|
|
142
|
+
)
|
|
143
|
+
return _rasterise(
|
|
144
|
+
points, values,
|
|
145
|
+
bounds=self._bounds, resolution=self._resolution,
|
|
146
|
+
reduce=self._reduce, background=self._background,
|
|
147
|
+
)
|
|
148
|
+
|
|
149
|
+
|
|
150
|
+
def to_uint8_image(
|
|
151
|
+
grid: np.ndarray, *, vmin: float | None = None, vmax: float | None = None
|
|
152
|
+
) -> np.ndarray:
|
|
153
|
+
"""Normalise a ``(H, W)`` grid to an ``(H, W, 3)`` uint8 image.
|
|
154
|
+
|
|
155
|
+
Pass fixed ``vmin``/``vmax`` for anything beyond one-off visualisation:
|
|
156
|
+
left at ``None``, each call normalises to its *own* min/max, so the same
|
|
157
|
+
physical value maps to a different pixel intensity from one frame to the
|
|
158
|
+
next -- fine to eyeball a single grid, wrong for a dataset a model trains
|
|
159
|
+
on (a given value must mean the same thing in every frame).
|
|
160
|
+
"""
|
|
161
|
+
grid = np.asarray(grid, dtype=np.float64)
|
|
162
|
+
lo = grid.min() if vmin is None else vmin
|
|
163
|
+
hi = grid.max() if vmax is None else vmax
|
|
164
|
+
norm = (grid - lo) / (hi - lo) if hi > lo else np.zeros_like(grid)
|
|
165
|
+
gray = np.clip(norm * 255.0, 0, 255).astype(np.uint8)
|
|
166
|
+
return np.repeat(gray[:, :, None], 3, axis=2)
|