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.
Files changed (83) hide show
  1. {euler_loading-2.29.0 → euler_loading-2.31.0}/CHANGELOG.md +29 -0
  2. {euler_loading-2.29.0 → euler_loading-2.31.0}/PKG-INFO +1 -1
  3. {euler_loading-2.29.0 → euler_loading-2.31.0}/docs/loaders.md +25 -3
  4. euler_loading-2.31.0/euler_loading/loaders/_princeton_dense.py +582 -0
  5. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/_writer_utils.py +63 -1
  6. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/cpu/generic_dense_depth.py +2 -1
  7. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/cpu/princeton_dense.py +18 -4
  8. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/cpu/synscapes.py +5 -47
  9. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/cpu/vkitti2.py +2 -1
  10. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/generate/loaders.json +4 -2
  11. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/gpu/generic_dense_depth.py +2 -1
  12. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/gpu/princeton_dense.py +18 -4
  13. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/gpu/vkitti2.py +2 -1
  14. {euler_loading-2.29.0 → euler_loading-2.31.0}/pyproject.toml +1 -1
  15. {euler_loading-2.29.0 → euler_loading-2.31.0}/tests/test_loaders.py +36 -5
  16. euler_loading-2.31.0/tests/test_princeton_dense.py +325 -0
  17. {euler_loading-2.29.0 → euler_loading-2.31.0}/tests/test_writing.py +16 -0
  18. euler_loading-2.29.0/euler_loading/loaders/_princeton_dense.py +0 -314
  19. {euler_loading-2.29.0 → euler_loading-2.31.0}/.gitignore +0 -0
  20. {euler_loading-2.29.0 → euler_loading-2.31.0}/LICENSE +0 -0
  21. {euler_loading-2.29.0 → euler_loading-2.31.0}/README.md +0 -0
  22. {euler_loading-2.29.0 → euler_loading-2.31.0}/docs/README.md +0 -0
  23. {euler_loading-2.29.0 → euler_loading-2.31.0}/docs/dataset.md +0 -0
  24. {euler_loading-2.29.0 → euler_loading-2.31.0}/docs/materialization.md +0 -0
  25. {euler_loading-2.29.0 → euler_loading-2.31.0}/docs/preprocessing.md +0 -0
  26. {euler_loading-2.29.0 → euler_loading-2.31.0}/docs/transform-descriptors.md +0 -0
  27. {euler_loading-2.29.0 → euler_loading-2.31.0}/docs/writing.md +0 -0
  28. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/__init__.py +0 -0
  29. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/__main__.py +0 -0
  30. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/_dataset_contract.py +0 -0
  31. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/_ds_crawler_utils.py +0 -0
  32. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/_metadata.py +0 -0
  33. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/_resolution.py +0 -0
  34. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/_writing.py +0 -0
  35. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/cli.py +0 -0
  36. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/dataset.py +0 -0
  37. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/dry_run.py +0 -0
  38. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/geometry.py +0 -0
  39. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/indexing.py +0 -0
  40. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/__init__.py +0 -0
  41. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/_annotations.py +0 -0
  42. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/_sky.py +0 -0
  43. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/contracts.py +0 -0
  44. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/cpu/__init__.py +0 -0
  45. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/cpu/generic.py +0 -0
  46. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/cpu/muses.py +0 -0
  47. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/cpu/real_drive_sim.py +0 -0
  48. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/generate/__init__.py +0 -0
  49. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/generate/__main__.py +0 -0
  50. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/generic.py +0 -0
  51. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/gpu/__init__.py +0 -0
  52. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/gpu/generic.py +0 -0
  53. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/gpu/muses.py +0 -0
  54. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/gpu/real_drive_sim.py +0 -0
  55. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/gpu/synscapes.py +0 -0
  56. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/materialized.py +0 -0
  57. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/muses.py +0 -0
  58. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/princeton_dense.py +0 -0
  59. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/real_drive_sim.py +0 -0
  60. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/synscapes.py +0 -0
  61. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/loaders/vkitti2.py +0 -0
  62. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/materialization.py +0 -0
  63. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/output_encoding.py +0 -0
  64. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/preprocessing.py +0 -0
  65. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/receipts.py +0 -0
  66. {euler_loading-2.29.0 → euler_loading-2.31.0}/euler_loading/transform_descriptors.py +0 -0
  67. {euler_loading-2.29.0 → euler_loading-2.31.0}/examples/README.md +0 -0
  68. {euler_loading-2.29.0 → euler_loading-2.31.0}/examples/real_drive_sim_preview.py +0 -0
  69. {euler_loading-2.29.0 → euler_loading-2.31.0}/examples/vkitti2_sample.py +0 -0
  70. {euler_loading-2.29.0 → euler_loading-2.31.0}/tests/__init__.py +0 -0
  71. {euler_loading-2.29.0 → euler_loading-2.31.0}/tests/conftest.py +0 -0
  72. {euler_loading-2.29.0 → euler_loading-2.31.0}/tests/example_rds_calib.json +0 -0
  73. {euler_loading-2.29.0 → euler_loading-2.31.0}/tests/test_dataset.py +0 -0
  74. {euler_loading-2.29.0 → euler_loading-2.31.0}/tests/test_dry_run.py +0 -0
  75. {euler_loading-2.29.0 → euler_loading-2.31.0}/tests/test_id_schema.py +0 -0
  76. {euler_loading-2.29.0 → euler_loading-2.31.0}/tests/test_indexing.py +0 -0
  77. {euler_loading-2.29.0 → euler_loading-2.31.0}/tests/test_materialization.py +0 -0
  78. {euler_loading-2.29.0 → euler_loading-2.31.0}/tests/test_preprocessing.py +0 -0
  79. {euler_loading-2.29.0 → euler_loading-2.31.0}/tests/test_python_compat.py +0 -0
  80. {euler_loading-2.29.0 → euler_loading-2.31.0}/tests/test_real_dataset.py +0 -0
  81. {euler_loading-2.29.0 → euler_loading-2.31.0}/tests/test_sky_class.py +0 -0
  82. {euler_loading-2.29.0 → euler_loading-2.31.0}/tests/test_synscapes.py +0 -0
  83. {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.29.0
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 (`GBRG`) TIFF debayered to RGB, `[0, 1]` |
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 | `K` from `calib_cam_stereo_left.json` · *hierarchical* |
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)