euler-loading 2.29.0__tar.gz → 2.31.0__tar.gz
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.
- {euler_loading-2.29.0 → euler_loading-2.31.0}/CHANGELOG.md +29 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/PKG-INFO +1 -1
- {euler_loading-2.29.0 → euler_loading-2.31.0}/docs/loaders.md +25 -3
- euler_loading-2.31.0/euler_loading/loaders/_princeton_dense.py +582 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/_writer_utils.py +63 -1
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/cpu/generic_dense_depth.py +2 -1
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/cpu/princeton_dense.py +18 -4
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/cpu/synscapes.py +5 -47
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/cpu/vkitti2.py +2 -1
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/generate/loaders.json +4 -2
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/gpu/generic_dense_depth.py +2 -1
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/gpu/princeton_dense.py +18 -4
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/gpu/vkitti2.py +2 -1
- {euler_loading-2.29.0 → euler_loading-2.31.0}/pyproject.toml +1 -1
- {euler_loading-2.29.0 → euler_loading-2.31.0}/tests/test_loaders.py +36 -5
- euler_loading-2.31.0/tests/test_princeton_dense.py +325 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/tests/test_writing.py +16 -0
- euler_loading-2.29.0/euler_loading/loaders/_princeton_dense.py +0 -314
- {euler_loading-2.29.0 → euler_loading-2.31.0}/.gitignore +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/LICENSE +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/README.md +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/docs/README.md +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/docs/dataset.md +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/docs/materialization.md +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/docs/preprocessing.md +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/docs/transform-descriptors.md +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/docs/writing.md +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/__init__.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/__main__.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/_dataset_contract.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/_ds_crawler_utils.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/_metadata.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/_resolution.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/_writing.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/cli.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/dataset.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/dry_run.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/geometry.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/indexing.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/__init__.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/_annotations.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/_sky.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/contracts.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/cpu/__init__.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/cpu/generic.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/cpu/muses.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/cpu/real_drive_sim.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/generate/__init__.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/generate/__main__.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/generic.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/gpu/__init__.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/gpu/generic.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/gpu/muses.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/gpu/real_drive_sim.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/gpu/synscapes.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/materialized.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/muses.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/princeton_dense.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/real_drive_sim.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/synscapes.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/vkitti2.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/materialization.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/output_encoding.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/preprocessing.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/receipts.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/transform_descriptors.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/examples/README.md +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/examples/real_drive_sim_preview.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/examples/vkitti2_sample.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/tests/__init__.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/tests/conftest.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/tests/example_rds_calib.json +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/tests/test_dataset.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/tests/test_dry_run.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/tests/test_id_schema.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/tests/test_indexing.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/tests/test_materialization.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/tests/test_preprocessing.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/tests/test_python_compat.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/tests/test_real_dataset.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/tests/test_sky_class.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/tests/test_synscapes.py +0 -0
- {euler_loading-2.29.0 → euler_loading-2.31.0}/tests/test_transform_descriptors.py +0 -0
|
@@ -2,6 +2,35 @@
|
|
|
2
2
|
|
|
3
3
|
## Unreleased
|
|
4
4
|
|
|
5
|
+
- **Behaviour change:** `princeton_dense.read_intrinsics` returns the
|
|
6
|
+
rectified pinhole matrix `P[:, :3]` from `calib_cam_stereo_left.json`
|
|
7
|
+
instead of the raw `K`. The `cam_stereo_left_lut` PNGs that
|
|
8
|
+
`princeton_dense.rgb` loads are rectified to `P` -- the official
|
|
9
|
+
SeeingThroughFog conversion remaps them with
|
|
10
|
+
`cv2.initUndistortRectifyMap(K, D, R, P)`, and the official lidar projection
|
|
11
|
+
uses `P` -- so `K` put projected lidar about 22 px off at the principal
|
|
12
|
+
point and up to 130 px off at the image edges. Recompute anything derived
|
|
13
|
+
from the old intrinsics, such as lidar projected into those images or depth
|
|
14
|
+
unprojected with them. `intrinsics_matrix: "K"` in the per-file attributes
|
|
15
|
+
or the head's meta selects the raw matrix, which belongs to the unrectified
|
|
16
|
+
12-bit TIFFs together with the distortion `D`.
|
|
17
|
+
- Add `princeton_dense.write_intrinsics`. It writes the matrix
|
|
18
|
+
`read_intrinsics` returns into a ROS camera-info JSON. An existing file is
|
|
19
|
+
updated rather than replaced: a crop or rescale of its stored matrix is
|
|
20
|
+
applied to `K` and `P` alike (whole-pixel crops as exact integer shifts),
|
|
21
|
+
`D`, `R`, `roi` and the header are kept, and `meta["width"]` /
|
|
22
|
+
`meta["height"]` set the recorded size. Any other change is refused. New
|
|
23
|
+
files and stream targets get an ideal pinhole camera.
|
|
24
|
+
- **Behaviour change:** `princeton_dense.rccb` demosaics the raw 12-bit
|
|
25
|
+
TIFFs as `GRBG` instead of `GBRG`, which swapped red and blue (red tail
|
|
26
|
+
lights came out blue). The official conversion uses OpenCV's
|
|
27
|
+
`COLOR_BAYER_GB2BGR`, and OpenCV names Bayer patterns after the sensor's
|
|
28
|
+
second row. `bayer_pattern` in the attributes or meta still overrides it.
|
|
29
|
+
|
|
30
|
+
- Fix `vkitti2.write_depth` and the 16-bit PNG path of
|
|
31
|
+
`generic_dense_depth.write_depth` on Pillow 13, which rejects
|
|
32
|
+
`Image.fromarray(uint16, mode="I;16")`; a uint16 array becomes an `I;16`
|
|
33
|
+
image without the argument on every supported Pillow.
|
|
5
34
|
- `Modality.function` picks a callable from the loader module the dataset head
|
|
6
35
|
names, instead of its `function`; the writer follows it.
|
|
7
36
|
`Modality(path, function="sky_mask")` reads a `class_segmentation` index as
|
|
@@ -1,6 +1,6 @@
|
|
|
1
1
|
Metadata-Version: 2.5
|
|
2
2
|
Name: euler-loading
|
|
3
|
-
Version: 2.
|
|
3
|
+
Version: 2.31.0
|
|
4
4
|
Summary: Multi-modal PyTorch dataloader using ds-crawler indices
|
|
5
5
|
Project-URL: Homepage, https://github.com/d-rothen/euler-loading
|
|
6
6
|
Project-URL: Repository, https://github.com/d-rothen/euler-loading
|
|
@@ -461,12 +461,34 @@ same resolution. Loaders preserve the stored resolution.
|
|
|
461
461
|
| Function | Shape | dtype | Notes |
|
|
462
462
|
|---|---|---|---|
|
|
463
463
|
| `rgb` | CHW / HWC | float32 | Plain 8-bit PNG, `[0, 1]` |
|
|
464
|
-
| `rccb` | CHW / HWC | float32 | 12-bit Bayer (`
|
|
464
|
+
| `rccb` | CHW / HWC | float32 | 12-bit Bayer (`GRBG`) TIFF debayered to RGB, `[0, 1]`; raw and unrectified |
|
|
465
465
|
| `sparse_depth` | (N, 5) | float32 | `.bin`: `x, y, z, intensity, ring` |
|
|
466
|
-
| `read_intrinsics` | 3×3 | float32 | `
|
|
466
|
+
| `read_intrinsics` | 3×3 | float32 | Rectified pinhole `P[:, :3]` from `calib_cam_stereo_left.json`; raw `K` with `intrinsics_matrix: "K"` · *hierarchical* |
|
|
467
467
|
| `read_extrinsics` | 4×4 | float32 | `lidar_hdl64_s3_roof` → `cam_stereo_left_optical`, from `calib_tf_tree_full.json` · *hierarchical* |
|
|
468
468
|
|
|
469
|
-
`lidar_point_cloud` and `point_cloud` are aliases for `sparse_depth`.
|
|
469
|
+
`lidar_point_cloud` and `point_cloud` are aliases for `sparse_depth`. Writers
|
|
470
|
+
exist for `rgb` and `read_intrinsics`.
|
|
471
|
+
|
|
472
|
+
The 8-bit `cam_stereo_left_lut` images that `rgb` reads are rectified: the
|
|
473
|
+
official SeeingThroughFog conversion remaps the raw frames with
|
|
474
|
+
`cv2.initUndistortRectifyMap(K, D, R, P)`, and the official lidar projection
|
|
475
|
+
uses `P`. `read_intrinsics` therefore returns `P[:, :3]`, an ideal pinhole
|
|
476
|
+
matrix with no distortion; `P[:, 3]` (zero for the left camera, the baseline
|
|
477
|
+
term for the right one) is not part of it. Set `intrinsics_matrix` to `"K"` in
|
|
478
|
+
the per-file attributes or the head's meta for the raw camera matrix of the
|
|
479
|
+
12-bit `cam_stereo_left` TIFFs (`rccb`) instead. It describes them only
|
|
480
|
+
together with the distortion `D`, which is large (`k1 ≈ -0.57`).
|
|
481
|
+
|
|
482
|
+
`write_intrinsics` writes that matrix back into the camera-info JSON. An
|
|
483
|
+
existing file is updated rather than replaced. The change from its stored
|
|
484
|
+
matrix must be a crop or a rescale, and it is applied to `K` and `P` alike, so
|
|
485
|
+
the two stay consistent; a whole-pixel crop shifts the stored values by exact
|
|
486
|
+
integers. `D`, `R`, `roi` and the header are kept, and `meta["width"]` and
|
|
487
|
+
`meta["height"]` set the recorded image size. A new file or a stream target
|
|
488
|
+
gets an ideal pinhole camera: `K = P[:, :3]`, zero distortion, identity `R`.
|
|
489
|
+
The extrinsics need no counterpart: a crop or rescale only changes the pixel
|
|
490
|
+
grid, so lidar projected through the written matrix lands on the cropped
|
|
491
|
+
pixels unchanged.
|
|
470
492
|
|
|
471
493
|
### Generic dense depth — `generic_dense_depth`
|
|
472
494
|
|
|
@@ -0,0 +1,582 @@
|
|
|
1
|
+
"""Shared helpers for Princeton DENSE / SeeingThroughFog loaders."""
|
|
2
|
+
|
|
3
|
+
from __future__ import annotations
|
|
4
|
+
|
|
5
|
+
import json
|
|
6
|
+
import logging
|
|
7
|
+
import math
|
|
8
|
+
import numbers
|
|
9
|
+
import os
|
|
10
|
+
from collections import deque
|
|
11
|
+
from collections.abc import Mapping
|
|
12
|
+
from typing import Any, BinaryIO, Union
|
|
13
|
+
|
|
14
|
+
import numpy as np
|
|
15
|
+
from PIL import Image
|
|
16
|
+
|
|
17
|
+
from euler_loading.loaders._writer_utils import (
|
|
18
|
+
mark_stream_supported,
|
|
19
|
+
replace_json,
|
|
20
|
+
to_numpy,
|
|
21
|
+
)
|
|
22
|
+
|
|
23
|
+
logger = logging.getLogger(__name__)
|
|
24
|
+
|
|
25
|
+
# The 2x2 colour layout from the sensor's top-left pixel. The official
|
|
26
|
+
# SeeingThroughFog conversion demosaics with OpenCV's COLOR_BAYER_GB2BGR, and
|
|
27
|
+
# OpenCV names patterns after the *second* row, so that is a GRBG sensor:
|
|
28
|
+
# G R / B G. Reading it as GBRG swaps red and blue.
|
|
29
|
+
_DEFAULT_BAYER_PATTERN = "GRBG"
|
|
30
|
+
_DEFAULT_RAW_MAX_VALUE = 4095.0
|
|
31
|
+
DEFAULT_CAMERA_FRAME = "cam_stereo_left_optical"
|
|
32
|
+
DEFAULT_LIDAR_FRAME = "lidar_hdl64_s3_roof"
|
|
33
|
+
LIDAR_COLUMNS = ["x", "y", "z", "intensity", "ring"]
|
|
34
|
+
DEFAULT_INTRINSICS_MATRIX = "P"
|
|
35
|
+
_INTRINSICS_MATRIX_ALIASES = {"p": "P", "rectified": "P", "k": "K", "raw": "K"}
|
|
36
|
+
_CAMERA_INFO_SHAPES = {"K": (3, 3), "P": (3, 4)}
|
|
37
|
+
|
|
38
|
+
|
|
39
|
+
def _value_from_context(
|
|
40
|
+
meta: Mapping[str, Any] | None,
|
|
41
|
+
attributes: Mapping[str, Any] | None,
|
|
42
|
+
keys: tuple[str, ...],
|
|
43
|
+
) -> Any:
|
|
44
|
+
for source in (attributes, meta):
|
|
45
|
+
if not isinstance(source, Mapping):
|
|
46
|
+
continue
|
|
47
|
+
for key in keys:
|
|
48
|
+
if key in source:
|
|
49
|
+
return source[key]
|
|
50
|
+
return None
|
|
51
|
+
|
|
52
|
+
|
|
53
|
+
def _rgb_scale(
|
|
54
|
+
raw: np.ndarray,
|
|
55
|
+
meta: Mapping[str, Any] | None,
|
|
56
|
+
attributes: Mapping[str, Any] | None,
|
|
57
|
+
) -> float:
|
|
58
|
+
value = _value_from_context(
|
|
59
|
+
meta,
|
|
60
|
+
attributes,
|
|
61
|
+
("rgb_max_value", "raw_max_value", "white_level"),
|
|
62
|
+
)
|
|
63
|
+
if value is not None:
|
|
64
|
+
return float(value)
|
|
65
|
+
|
|
66
|
+
if np.issubdtype(raw.dtype, np.integer):
|
|
67
|
+
if raw.dtype == np.uint8:
|
|
68
|
+
return 255.0
|
|
69
|
+
if raw.size and float(raw.max()) <= _DEFAULT_RAW_MAX_VALUE:
|
|
70
|
+
return _DEFAULT_RAW_MAX_VALUE
|
|
71
|
+
return float(np.iinfo(raw.dtype).max)
|
|
72
|
+
|
|
73
|
+
return 1.0
|
|
74
|
+
|
|
75
|
+
|
|
76
|
+
def _bayer_pattern(
|
|
77
|
+
meta: Mapping[str, Any] | None,
|
|
78
|
+
attributes: Mapping[str, Any] | None,
|
|
79
|
+
) -> str:
|
|
80
|
+
value = _value_from_context(meta, attributes, ("bayer_pattern", "cfa_pattern"))
|
|
81
|
+
if value is None:
|
|
82
|
+
return _DEFAULT_BAYER_PATTERN
|
|
83
|
+
pattern = str(value).upper()
|
|
84
|
+
if len(pattern) != 4 or any(channel not in "RGB" for channel in pattern):
|
|
85
|
+
raise ValueError("bayer_pattern must be a 4-character pattern using R, G, and B")
|
|
86
|
+
return pattern
|
|
87
|
+
|
|
88
|
+
|
|
89
|
+
def _interpolate_channel(values: np.ndarray, mask: np.ndarray) -> np.ndarray:
|
|
90
|
+
values_pad = np.pad(values, 1, mode="edge")
|
|
91
|
+
mask_pad = np.pad(mask.astype(np.float32), 1, mode="edge")
|
|
92
|
+
|
|
93
|
+
sums = np.zeros_like(values, dtype=np.float32)
|
|
94
|
+
counts = np.zeros_like(values, dtype=np.float32)
|
|
95
|
+
for y_offset in range(3):
|
|
96
|
+
for x_offset in range(3):
|
|
97
|
+
y = slice(y_offset, y_offset + values.shape[0])
|
|
98
|
+
x = slice(x_offset, x_offset + values.shape[1])
|
|
99
|
+
sums += values_pad[y, x]
|
|
100
|
+
counts += mask_pad[y, x]
|
|
101
|
+
|
|
102
|
+
interpolated = np.divide(
|
|
103
|
+
sums,
|
|
104
|
+
counts,
|
|
105
|
+
out=np.zeros_like(sums, dtype=np.float32),
|
|
106
|
+
where=counts > 0,
|
|
107
|
+
)
|
|
108
|
+
return np.where(mask, values, interpolated)
|
|
109
|
+
|
|
110
|
+
|
|
111
|
+
def _demosaic_bayer(raw: np.ndarray, pattern: str) -> np.ndarray:
|
|
112
|
+
raw_f32 = raw.astype(np.float32, copy=False)
|
|
113
|
+
channel_values = {
|
|
114
|
+
"R": np.zeros(raw.shape, dtype=np.float32),
|
|
115
|
+
"G": np.zeros(raw.shape, dtype=np.float32),
|
|
116
|
+
"B": np.zeros(raw.shape, dtype=np.float32),
|
|
117
|
+
}
|
|
118
|
+
channel_masks = {
|
|
119
|
+
"R": np.zeros(raw.shape, dtype=bool),
|
|
120
|
+
"G": np.zeros(raw.shape, dtype=bool),
|
|
121
|
+
"B": np.zeros(raw.shape, dtype=bool),
|
|
122
|
+
}
|
|
123
|
+
|
|
124
|
+
positions = (
|
|
125
|
+
(slice(0, None, 2), slice(0, None, 2), pattern[0]),
|
|
126
|
+
(slice(0, None, 2), slice(1, None, 2), pattern[1]),
|
|
127
|
+
(slice(1, None, 2), slice(0, None, 2), pattern[2]),
|
|
128
|
+
(slice(1, None, 2), slice(1, None, 2), pattern[3]),
|
|
129
|
+
)
|
|
130
|
+
for row_slice, col_slice, channel in positions:
|
|
131
|
+
channel_values[channel][row_slice, col_slice] = raw_f32[row_slice, col_slice]
|
|
132
|
+
channel_masks[channel][row_slice, col_slice] = True
|
|
133
|
+
|
|
134
|
+
channels = [
|
|
135
|
+
_interpolate_channel(channel_values[channel], channel_masks[channel])
|
|
136
|
+
for channel in ("R", "G", "B")
|
|
137
|
+
]
|
|
138
|
+
return np.stack(channels, axis=-1)
|
|
139
|
+
|
|
140
|
+
|
|
141
|
+
def load_rgb_array(
|
|
142
|
+
path: Union[str, BinaryIO],
|
|
143
|
+
meta: Mapping[str, Any] | None = None,
|
|
144
|
+
attributes: Mapping[str, Any] | None = None,
|
|
145
|
+
) -> np.ndarray:
|
|
146
|
+
"""Load a SeeingThroughFog RGB TIFF as an ``(H, W, 3)`` float32 array."""
|
|
147
|
+
with Image.open(path) as image:
|
|
148
|
+
raw = np.array(image)
|
|
149
|
+
|
|
150
|
+
if raw.ndim == 2:
|
|
151
|
+
rgb = _demosaic_bayer(raw, _bayer_pattern(meta, attributes))
|
|
152
|
+
scale = _rgb_scale(raw, meta, attributes)
|
|
153
|
+
elif raw.ndim == 3 and raw.shape[-1] >= 3:
|
|
154
|
+
rgb = raw[..., :3].astype(np.float32, copy=False)
|
|
155
|
+
scale = _rgb_scale(raw, meta, attributes)
|
|
156
|
+
else:
|
|
157
|
+
name = getattr(path, "name", path)
|
|
158
|
+
raise ValueError(f"Unsupported Princeton RGB image shape for {name!r}: {raw.shape}")
|
|
159
|
+
|
|
160
|
+
if scale <= 0:
|
|
161
|
+
raise ValueError(f"RGB scale must be positive, got {scale!r}")
|
|
162
|
+
return np.clip(rgb / scale, 0.0, 1.0).astype(np.float32, copy=False)
|
|
163
|
+
|
|
164
|
+
|
|
165
|
+
def load_sparse_depth_array(path: Union[str, BinaryIO]) -> np.ndarray:
|
|
166
|
+
"""Load a SeeingThroughFog lidar ``.bin`` file as ``(N, 5)`` float32."""
|
|
167
|
+
if isinstance(path, (str, os.PathLike)):
|
|
168
|
+
values = np.fromfile(path, dtype=np.float32)
|
|
169
|
+
else:
|
|
170
|
+
values = np.frombuffer(path.read(), dtype=np.float32).copy()
|
|
171
|
+
|
|
172
|
+
if values.size % 5 != 0:
|
|
173
|
+
name = getattr(path, "name", path)
|
|
174
|
+
raise ValueError(
|
|
175
|
+
f"Princeton sparse-depth file {name!r} contains {values.size} "
|
|
176
|
+
"float32 values, which is not divisible by 5."
|
|
177
|
+
)
|
|
178
|
+
return values.reshape((-1, 5))
|
|
179
|
+
|
|
180
|
+
|
|
181
|
+
def _load_json(path: Union[str, BinaryIO]) -> Any:
|
|
182
|
+
if isinstance(path, (str, os.PathLike)):
|
|
183
|
+
with open(path) as f:
|
|
184
|
+
return json.load(f)
|
|
185
|
+
return json.load(path)
|
|
186
|
+
|
|
187
|
+
|
|
188
|
+
def intrinsics_matrix(
|
|
189
|
+
meta: Mapping[str, Any] | None,
|
|
190
|
+
attributes: Mapping[str, Any] | None,
|
|
191
|
+
) -> str:
|
|
192
|
+
"""Return which camera-info matrix the intrinsics come from: ``"P"`` or ``"K"``.
|
|
193
|
+
|
|
194
|
+
``intrinsics_matrix`` is read from the per-file attributes, then from the
|
|
195
|
+
dataset head's meta. ``"P"`` (alias ``"rectified"``, the default) selects
|
|
196
|
+
the rectified projection, ``"K"`` (alias ``"raw"``) the raw camera matrix.
|
|
197
|
+
"""
|
|
198
|
+
value = _value_from_context(meta, attributes, ("intrinsics_matrix",))
|
|
199
|
+
if value is None:
|
|
200
|
+
return DEFAULT_INTRINSICS_MATRIX
|
|
201
|
+
selected = _INTRINSICS_MATRIX_ALIASES.get(str(value).strip().lower())
|
|
202
|
+
if selected is None:
|
|
203
|
+
raise ValueError(
|
|
204
|
+
"intrinsics_matrix must be 'P' (rectified) or 'K' (raw), "
|
|
205
|
+
f"got {value!r}"
|
|
206
|
+
)
|
|
207
|
+
return selected
|
|
208
|
+
|
|
209
|
+
|
|
210
|
+
def _camera_info_matrix(
|
|
211
|
+
data: Mapping[str, Any], key: str, name: Any
|
|
212
|
+
) -> np.ndarray | None:
|
|
213
|
+
"""Return camera-info ``K`` (3x3) or ``P`` (3x4) as float64, or ``None``."""
|
|
214
|
+
raw = data.get(key)
|
|
215
|
+
if raw is None:
|
|
216
|
+
return None
|
|
217
|
+
shape = _CAMERA_INFO_SHAPES[key]
|
|
218
|
+
matrix = np.asarray(raw, dtype=np.float64)
|
|
219
|
+
if matrix.shape == (shape[0] * shape[1],):
|
|
220
|
+
matrix = matrix.reshape(shape)
|
|
221
|
+
if matrix.shape != shape:
|
|
222
|
+
raise ValueError(
|
|
223
|
+
f"Princeton camera {key} in {name!r} must have shape {shape}, "
|
|
224
|
+
f"got {matrix.shape}."
|
|
225
|
+
)
|
|
226
|
+
return matrix
|
|
227
|
+
|
|
228
|
+
|
|
229
|
+
def _check_pinhole(matrix: np.ndarray, what: str) -> None:
|
|
230
|
+
"""Raise unless *matrix* is a finite ``[[fx, s, cx], [0, fy, cy], [0, 0, 1]]``."""
|
|
231
|
+
if not np.all(np.isfinite(matrix)):
|
|
232
|
+
raise ValueError(f"{what} must be finite")
|
|
233
|
+
if (
|
|
234
|
+
not np.allclose(matrix[2], (0.0, 0.0, 1.0), rtol=0.0, atol=1e-9)
|
|
235
|
+
or abs(matrix[1, 0]) > 1e-9
|
|
236
|
+
or matrix[0, 0] <= 0.0
|
|
237
|
+
or matrix[1, 1] <= 0.0
|
|
238
|
+
):
|
|
239
|
+
raise ValueError(
|
|
240
|
+
f"{what} must be a pinhole camera matrix [[fx, s, cx], [0, fy, cy], "
|
|
241
|
+
f"[0, 0, 1]] with positive focal lengths, got {matrix.tolist()}"
|
|
242
|
+
)
|
|
243
|
+
|
|
244
|
+
|
|
245
|
+
def load_intrinsics_array(
|
|
246
|
+
path: Union[str, BinaryIO],
|
|
247
|
+
meta: Mapping[str, Any] | None = None,
|
|
248
|
+
attributes: Mapping[str, Any] | None = None,
|
|
249
|
+
) -> np.ndarray:
|
|
250
|
+
"""Load the left stereo camera's ``(3, 3)`` pinhole matrix from ROS camera-info JSON.
|
|
251
|
+
|
|
252
|
+
The default is the rectified projection's ``P[:, :3]``. The 8-bit
|
|
253
|
+
``cam_stereo_left_lut`` PNGs are rectified to ``P`` -- the official
|
|
254
|
+
conversion remaps them with ``cv2.initUndistortRectifyMap(K, D, R, P)`` --
|
|
255
|
+
so for those images it is an ideal pinhole camera without distortion, and
|
|
256
|
+
it is the matrix the official lidar projection uses. ``P[:, 3]``, zero for
|
|
257
|
+
the left camera and the stereo baseline term for the right one, is not part
|
|
258
|
+
of it.
|
|
259
|
+
|
|
260
|
+
``intrinsics_matrix: "K"`` in the per-file attributes or *meta* returns
|
|
261
|
+
the raw camera matrix ``K`` instead. It belongs to the unrectified 12-bit
|
|
262
|
+
TIFFs and describes them only together with the distortion ``D``.
|
|
263
|
+
"""
|
|
264
|
+
data = _load_json(path)
|
|
265
|
+
name = getattr(path, "name", path)
|
|
266
|
+
selected = intrinsics_matrix(meta, attributes)
|
|
267
|
+
matrix = (
|
|
268
|
+
_camera_info_matrix(data, selected, name) if isinstance(data, Mapping) else None
|
|
269
|
+
)
|
|
270
|
+
if matrix is None:
|
|
271
|
+
hint = (
|
|
272
|
+
" Set intrinsics_matrix to 'K' to read the raw camera matrix instead."
|
|
273
|
+
if selected == "P"
|
|
274
|
+
else ""
|
|
275
|
+
)
|
|
276
|
+
raise ValueError(
|
|
277
|
+
f"Princeton camera calibration {name!r} must contain a {selected} array.{hint}"
|
|
278
|
+
)
|
|
279
|
+
pinhole = matrix[:, :3]
|
|
280
|
+
_check_pinhole(pinhole, f"Princeton camera {selected} in {name!r}")
|
|
281
|
+
return pinhole.astype(np.float32)
|
|
282
|
+
|
|
283
|
+
|
|
284
|
+
def _image_size(meta: Mapping[str, Any]) -> tuple[int, int] | None:
|
|
285
|
+
"""Return ``(width, height)`` from ``meta``, or ``None`` when neither is given."""
|
|
286
|
+
width, height = meta.get("width"), meta.get("height")
|
|
287
|
+
if width is None and height is None:
|
|
288
|
+
return None
|
|
289
|
+
size = []
|
|
290
|
+
for key, value in (("width", width), ("height", height)):
|
|
291
|
+
if (
|
|
292
|
+
isinstance(value, bool)
|
|
293
|
+
or not isinstance(value, numbers.Real)
|
|
294
|
+
or not math.isfinite(value)
|
|
295
|
+
or value <= 0
|
|
296
|
+
or int(value) != value
|
|
297
|
+
):
|
|
298
|
+
raise ValueError(
|
|
299
|
+
f"meta[{key!r}] must be a positive integer pixel count, got {value!r}"
|
|
300
|
+
)
|
|
301
|
+
size.append(int(value))
|
|
302
|
+
return size[0], size[1]
|
|
303
|
+
|
|
304
|
+
|
|
305
|
+
def _pixel_transform(transform: np.ndarray, name: str) -> np.ndarray:
|
|
306
|
+
"""Return the crop or rescale *transform* represents, or raise.
|
|
307
|
+
|
|
308
|
+
*transform* maps the stored matrix's pixel grid onto the new one. Only an
|
|
309
|
+
axis-aligned scale and shift can be applied to ``K`` and ``P`` alike. A
|
|
310
|
+
whole-pixel crop (unit scale, integer shift) is snapped to exact values, so
|
|
311
|
+
the stored float64 entries are shifted without float32 rounding noise.
|
|
312
|
+
"""
|
|
313
|
+
tolerance = 1e-6
|
|
314
|
+
if (
|
|
315
|
+
not np.all(np.isfinite(transform))
|
|
316
|
+
or abs(transform[0, 1]) > tolerance
|
|
317
|
+
or abs(transform[1, 0]) > tolerance
|
|
318
|
+
or not np.allclose(transform[2], (0.0, 0.0, 1.0), rtol=0.0, atol=tolerance)
|
|
319
|
+
or transform[0, 0] <= 0.0
|
|
320
|
+
or transform[1, 1] <= 0.0
|
|
321
|
+
):
|
|
322
|
+
raise ValueError(
|
|
323
|
+
f"Refusing to update {name!r}: the new intrinsics are not a crop or "
|
|
324
|
+
"rescale of its stored matrix, so its K and P cannot be kept "
|
|
325
|
+
"consistent. Write them to a new file instead."
|
|
326
|
+
)
|
|
327
|
+
exact = np.eye(3)
|
|
328
|
+
exact[0, 0], exact[1, 1] = transform[0, 0], transform[1, 1]
|
|
329
|
+
exact[:2, 2] = transform[:2, 2]
|
|
330
|
+
shift = np.round(exact[:2, 2])
|
|
331
|
+
if (
|
|
332
|
+
abs(exact[0, 0] - 1.0) <= tolerance
|
|
333
|
+
and abs(exact[1, 1] - 1.0) <= tolerance
|
|
334
|
+
and np.all(np.abs(exact[:2, 2] - shift) <= 1e-3)
|
|
335
|
+
):
|
|
336
|
+
exact[0, 0] = exact[1, 1] = 1.0
|
|
337
|
+
exact[:2, 2] = shift
|
|
338
|
+
return exact
|
|
339
|
+
|
|
340
|
+
|
|
341
|
+
def _update_camera_info(
|
|
342
|
+
document: dict[str, Any],
|
|
343
|
+
matrix: np.ndarray,
|
|
344
|
+
selected: str,
|
|
345
|
+
size: tuple[int, int] | None,
|
|
346
|
+
name: str,
|
|
347
|
+
) -> None:
|
|
348
|
+
"""Move *document*'s ``K`` and ``P`` onto the pixel grid of *matrix*."""
|
|
349
|
+
stored = {key: _camera_info_matrix(document, key, name) for key in _CAMERA_INFO_SHAPES}
|
|
350
|
+
reference = stored[selected]
|
|
351
|
+
if reference is None:
|
|
352
|
+
raise ValueError(
|
|
353
|
+
f"Refusing to update {name!r}: it has no {selected} matrix to update."
|
|
354
|
+
)
|
|
355
|
+
_check_pinhole(reference[:, :3], f"Princeton camera {selected} in {name!r}")
|
|
356
|
+
transform = _pixel_transform(matrix @ np.linalg.inv(reference[:, :3]), name)
|
|
357
|
+
for key, value in stored.items():
|
|
358
|
+
if value is not None:
|
|
359
|
+
# ``+ 0.0`` turns a -0.0 from the product into 0.0.
|
|
360
|
+
document[key] = ((transform @ value) + 0.0).reshape(-1).tolist()
|
|
361
|
+
if size is not None:
|
|
362
|
+
document["width"], document["height"] = size
|
|
363
|
+
elif not np.array_equal(transform, np.eye(3)) and (
|
|
364
|
+
"width" in document or "height" in document
|
|
365
|
+
):
|
|
366
|
+
logger.warning(
|
|
367
|
+
"Updated the camera matrices in %r without meta['width'] and "
|
|
368
|
+
"meta['height']; its recorded %sx%s image size may no longer match.",
|
|
369
|
+
name,
|
|
370
|
+
document.get("width"),
|
|
371
|
+
document.get("height"),
|
|
372
|
+
)
|
|
373
|
+
|
|
374
|
+
|
|
375
|
+
def _pinhole_camera_info(
|
|
376
|
+
matrix: np.ndarray, meta: Mapping[str, Any], size: tuple[int, int] | None
|
|
377
|
+
) -> dict[str, Any]:
|
|
378
|
+
"""Return a camera-info document for an ideal pinhole camera *matrix*."""
|
|
379
|
+
frame = meta.get("camera_frame")
|
|
380
|
+
document: dict[str, Any] = {
|
|
381
|
+
"header": {
|
|
382
|
+
"frame_id": frame if isinstance(frame, str) and frame else DEFAULT_CAMERA_FRAME
|
|
383
|
+
},
|
|
384
|
+
"distortion_model": "plumb_bob",
|
|
385
|
+
"D": [0.0, 0.0, 0.0, 0.0, 0.0],
|
|
386
|
+
"K": matrix.reshape(-1).tolist(),
|
|
387
|
+
"R": np.eye(3).reshape(-1).tolist(),
|
|
388
|
+
"P": np.hstack([matrix, np.zeros((3, 1))]).reshape(-1).tolist(),
|
|
389
|
+
"binning_x": 0,
|
|
390
|
+
"binning_y": 0,
|
|
391
|
+
"roi": {"x_offset": 0, "y_offset": 0, "height": 0, "width": 0, "do_rectify": False},
|
|
392
|
+
}
|
|
393
|
+
if size is not None:
|
|
394
|
+
document["width"], document["height"] = size
|
|
395
|
+
return document
|
|
396
|
+
|
|
397
|
+
|
|
398
|
+
@mark_stream_supported
|
|
399
|
+
def write_intrinsics(
|
|
400
|
+
path: Union[str, os.PathLike, BinaryIO],
|
|
401
|
+
value: Any,
|
|
402
|
+
meta: Mapping[str, Any] | None = None,
|
|
403
|
+
) -> None:
|
|
404
|
+
"""Write a pinhole matrix into a ROS camera-info JSON (``calib_cam_stereo_left.json``).
|
|
405
|
+
|
|
406
|
+
*value* is what :func:`load_intrinsics_array` returns for the same *meta* --
|
|
407
|
+
the rectified ``P[:, :3]`` by default, the raw ``K`` with
|
|
408
|
+
``intrinsics_matrix: "K"`` -- so a written file reads back as *value*.
|
|
409
|
+
|
|
410
|
+
An existing file is updated rather than replaced. The change from its
|
|
411
|
+
stored matrix to *value* must be a crop or a rescale: an axis-aligned scale
|
|
412
|
+
and shift of the pixel grid. It is applied to both ``K`` and ``P``, so the
|
|
413
|
+
raw and rectified matrices stay consistent, as ROS ``image_geometry`` would
|
|
414
|
+
derive them for a region of interest. ``D``, ``R``, ``roi``, ``header`` and
|
|
415
|
+
all other fields are kept. A whole-pixel crop shifts the stored float64
|
|
416
|
+
values by exact integers. ``meta["width"]`` and ``meta["height"]`` set the
|
|
417
|
+
recorded image size; pass them whenever the matrix no longer matches it.
|
|
418
|
+
The update replaces the file in one step, as the Synscapes camera writers
|
|
419
|
+
do.
|
|
420
|
+
|
|
421
|
+
A new file, or a stream target (which cannot be read back), receives an
|
|
422
|
+
ideal pinhole camera: ``K = P[:, :3] = value``, zero distortion and an
|
|
423
|
+
identity ``R``.
|
|
424
|
+
"""
|
|
425
|
+
matrix = to_numpy(value).astype(np.float64)
|
|
426
|
+
if matrix.shape != (3, 3):
|
|
427
|
+
raise ValueError(f"intrinsics must have shape (3, 3), got {matrix.shape}")
|
|
428
|
+
_check_pinhole(matrix, "intrinsics")
|
|
429
|
+
options = meta if isinstance(meta, Mapping) else {}
|
|
430
|
+
selected = intrinsics_matrix(options, None)
|
|
431
|
+
size = _image_size(options)
|
|
432
|
+
|
|
433
|
+
if isinstance(path, (str, os.PathLike)) and os.path.exists(path):
|
|
434
|
+
name = os.fspath(path)
|
|
435
|
+
try:
|
|
436
|
+
document = _load_json(path)
|
|
437
|
+
except ValueError as exc:
|
|
438
|
+
raise ValueError(
|
|
439
|
+
f"Refusing to overwrite {name!r}: it exists but is not readable "
|
|
440
|
+
f"JSON, so its contents cannot be preserved ({exc})."
|
|
441
|
+
) from exc
|
|
442
|
+
if not isinstance(document, dict):
|
|
443
|
+
raise ValueError(
|
|
444
|
+
f"Refusing to overwrite {name!r}: expected a JSON object to merge "
|
|
445
|
+
f"into, found {type(document).__name__}."
|
|
446
|
+
)
|
|
447
|
+
_update_camera_info(document, matrix, selected, size, name)
|
|
448
|
+
else:
|
|
449
|
+
document = _pinhole_camera_info(matrix, options, size)
|
|
450
|
+
replace_json(path, document, indent=4)
|
|
451
|
+
|
|
452
|
+
|
|
453
|
+
def _select_frame(
|
|
454
|
+
meta: Mapping[str, Any] | None,
|
|
455
|
+
attributes: Mapping[str, Any] | None,
|
|
456
|
+
keys: tuple[str, ...],
|
|
457
|
+
default: str,
|
|
458
|
+
) -> str:
|
|
459
|
+
value = _value_from_context(meta, attributes, keys)
|
|
460
|
+
if value is None:
|
|
461
|
+
return default
|
|
462
|
+
frame = str(value)
|
|
463
|
+
aliases = {
|
|
464
|
+
"camera": DEFAULT_CAMERA_FRAME,
|
|
465
|
+
"cam_stereo_left": DEFAULT_CAMERA_FRAME,
|
|
466
|
+
"left": DEFAULT_CAMERA_FRAME,
|
|
467
|
+
"rgb": DEFAULT_CAMERA_FRAME,
|
|
468
|
+
"lidar": DEFAULT_LIDAR_FRAME,
|
|
469
|
+
"hdl64": DEFAULT_LIDAR_FRAME,
|
|
470
|
+
"hdl64_s3": DEFAULT_LIDAR_FRAME,
|
|
471
|
+
"lidar_hdl64": DEFAULT_LIDAR_FRAME,
|
|
472
|
+
}
|
|
473
|
+
return aliases.get(frame, frame)
|
|
474
|
+
|
|
475
|
+
|
|
476
|
+
def _matrix_from_transform(transform: Mapping[str, Any]) -> np.ndarray:
|
|
477
|
+
translation = transform.get("translation")
|
|
478
|
+
rotation = transform.get("rotation")
|
|
479
|
+
if not isinstance(translation, Mapping) or not isinstance(rotation, Mapping):
|
|
480
|
+
raise ValueError("TF transform entries must contain translation and rotation objects.")
|
|
481
|
+
|
|
482
|
+
x = float(rotation.get("x", 0.0))
|
|
483
|
+
y = float(rotation.get("y", 0.0))
|
|
484
|
+
z = float(rotation.get("z", 0.0))
|
|
485
|
+
w = float(rotation.get("w", 1.0))
|
|
486
|
+
norm = float(np.sqrt(x * x + y * y + z * z + w * w))
|
|
487
|
+
if norm <= 0.0:
|
|
488
|
+
raise ValueError("TF quaternion must have non-zero norm.")
|
|
489
|
+
x /= norm
|
|
490
|
+
y /= norm
|
|
491
|
+
z /= norm
|
|
492
|
+
w /= norm
|
|
493
|
+
|
|
494
|
+
matrix = np.eye(4, dtype=np.float64)
|
|
495
|
+
matrix[:3, :3] = np.array(
|
|
496
|
+
[
|
|
497
|
+
[1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)],
|
|
498
|
+
[2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)],
|
|
499
|
+
[2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)],
|
|
500
|
+
],
|
|
501
|
+
dtype=np.float64,
|
|
502
|
+
)
|
|
503
|
+
matrix[:3, 3] = [
|
|
504
|
+
float(translation.get("x", 0.0)),
|
|
505
|
+
float(translation.get("y", 0.0)),
|
|
506
|
+
float(translation.get("z", 0.0)),
|
|
507
|
+
]
|
|
508
|
+
return matrix
|
|
509
|
+
|
|
510
|
+
|
|
511
|
+
def _tf_edges(data: Any) -> dict[str, list[tuple[str, np.ndarray]]]:
|
|
512
|
+
if not isinstance(data, list):
|
|
513
|
+
raise ValueError("Princeton TF calibration must be a JSON list of transforms.")
|
|
514
|
+
|
|
515
|
+
edges: dict[str, list[tuple[str, np.ndarray]]] = {}
|
|
516
|
+
for entry in data:
|
|
517
|
+
if not isinstance(entry, Mapping):
|
|
518
|
+
continue
|
|
519
|
+
header = entry.get("header")
|
|
520
|
+
child = entry.get("child_frame_id")
|
|
521
|
+
transform = entry.get("transform")
|
|
522
|
+
if not isinstance(header, Mapping) or not isinstance(child, str):
|
|
523
|
+
continue
|
|
524
|
+
parent = header.get("frame_id")
|
|
525
|
+
if not isinstance(parent, str) or not isinstance(transform, Mapping):
|
|
526
|
+
continue
|
|
527
|
+
|
|
528
|
+
# ROS TransformStamped stores the child pose in the parent frame.
|
|
529
|
+
# This matrix maps coordinates from child frame to parent frame.
|
|
530
|
+
parent_from_child = _matrix_from_transform(transform)
|
|
531
|
+
child_from_parent = np.linalg.inv(parent_from_child)
|
|
532
|
+
edges.setdefault(child, []).append((parent, parent_from_child))
|
|
533
|
+
edges.setdefault(parent, []).append((child, child_from_parent))
|
|
534
|
+
return edges
|
|
535
|
+
|
|
536
|
+
|
|
537
|
+
def _compose_tf(
|
|
538
|
+
edges: Mapping[str, list[tuple[str, np.ndarray]]],
|
|
539
|
+
source_frame: str,
|
|
540
|
+
target_frame: str,
|
|
541
|
+
) -> np.ndarray:
|
|
542
|
+
queue: deque[tuple[str, np.ndarray]] = deque(
|
|
543
|
+
[(source_frame, np.eye(4, dtype=np.float64))]
|
|
544
|
+
)
|
|
545
|
+
seen = {source_frame}
|
|
546
|
+
|
|
547
|
+
while queue:
|
|
548
|
+
frame, frame_from_source = queue.popleft()
|
|
549
|
+
if frame == target_frame:
|
|
550
|
+
return frame_from_source.astype(np.float32)
|
|
551
|
+
for next_frame, next_from_frame in edges.get(frame, []):
|
|
552
|
+
if next_frame in seen:
|
|
553
|
+
continue
|
|
554
|
+
seen.add(next_frame)
|
|
555
|
+
queue.append((next_frame, next_from_frame @ frame_from_source))
|
|
556
|
+
|
|
557
|
+
available = ", ".join(sorted(edges.keys()))
|
|
558
|
+
raise KeyError(
|
|
559
|
+
f"No TF path from {source_frame!r} to {target_frame!r}. "
|
|
560
|
+
f"Available frames: {available}"
|
|
561
|
+
)
|
|
562
|
+
|
|
563
|
+
|
|
564
|
+
def load_extrinsics_array(
|
|
565
|
+
path: Union[str, BinaryIO],
|
|
566
|
+
meta: Mapping[str, Any] | None = None,
|
|
567
|
+
attributes: Mapping[str, Any] | None = None,
|
|
568
|
+
) -> np.ndarray:
|
|
569
|
+
"""Load a 4x4 transform mapping HDL64 lidar points into the camera frame."""
|
|
570
|
+
source_frame = _select_frame(
|
|
571
|
+
meta,
|
|
572
|
+
attributes,
|
|
573
|
+
("source_frame", "source_sensor", "from_frame", "from_sensor", "source"),
|
|
574
|
+
DEFAULT_LIDAR_FRAME,
|
|
575
|
+
)
|
|
576
|
+
target_frame = _select_frame(
|
|
577
|
+
meta,
|
|
578
|
+
attributes,
|
|
579
|
+
("target_frame", "target_sensor", "to_frame", "to_sensor", "target", "camera"),
|
|
580
|
+
DEFAULT_CAMERA_FRAME,
|
|
581
|
+
)
|
|
582
|
+
return _compose_tf(_tf_edges(_load_json(path)), source_frame, target_frame)
|