camera-client 0.2.0__tar.gz → 0.2.1__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.
- {camera_client-0.2.0/camera_client.egg-info → camera_client-0.2.1}/PKG-INFO +3 -3
- {camera_client-0.2.0 → camera_client-0.2.1}/README.md +2 -2
- camera_client-0.2.1/camera_client/client.py +303 -0
- {camera_client-0.2.0 → camera_client-0.2.1/camera_client.egg-info}/PKG-INFO +3 -3
- {camera_client-0.2.0 → camera_client-0.2.1}/pyproject.toml +1 -1
- camera_client-0.2.0/camera_client/client.py +0 -724
- {camera_client-0.2.0 → camera_client-0.2.1}/LICENSE +0 -0
- {camera_client-0.2.0 → camera_client-0.2.1}/MANIFEST.in +0 -0
- {camera_client-0.2.0 → camera_client-0.2.1}/camera_client/__init__.py +0 -0
- {camera_client-0.2.0 → camera_client-0.2.1}/camera_client/loading.py +0 -0
- {camera_client-0.2.0 → camera_client-0.2.1}/camera_client.egg-info/SOURCES.txt +0 -0
- {camera_client-0.2.0 → camera_client-0.2.1}/camera_client.egg-info/dependency_links.txt +0 -0
- {camera_client-0.2.0 → camera_client-0.2.1}/camera_client.egg-info/entry_points.txt +0 -0
- {camera_client-0.2.0 → camera_client-0.2.1}/camera_client.egg-info/requires.txt +0 -0
- {camera_client-0.2.0 → camera_client-0.2.1}/camera_client.egg-info/top_level.txt +0 -0
- {camera_client-0.2.0 → camera_client-0.2.1}/requirements.txt +0 -0
- {camera_client-0.2.0 → camera_client-0.2.1}/setup.cfg +0 -0
- {camera_client-0.2.0 → camera_client-0.2.1}/setup.py +0 -0
|
@@ -1,6 +1,6 @@
|
|
|
1
1
|
Metadata-Version: 2.4
|
|
2
2
|
Name: camera-client
|
|
3
|
-
Version: 0.2.
|
|
3
|
+
Version: 0.2.1
|
|
4
4
|
Summary: Python SDK for camera calibration and projection transformations - handle lens distortion, coordinate transformations, and 3D ray casting with symbolic expressions.
|
|
5
5
|
Author-email: Alexander Abramov <extremal.ru@gmail.com>
|
|
6
6
|
License: MIT
|
|
@@ -178,7 +178,7 @@ rays = camera.ctd_to_ray(ctd_points)
|
|
|
178
178
|
|
|
179
179
|
# Get camera position in world space (ray origin)
|
|
180
180
|
key_point = camera.get_key_point()
|
|
181
|
-
print(key_point.shape) # (3,
|
|
181
|
+
print(key_point.shape) # (3,) - [x, y, z] camera position
|
|
182
182
|
|
|
183
183
|
# Ray equation: point_on_ray = key_point + t * ray_direction
|
|
184
184
|
# All rays are normalized to unit length
|
|
@@ -314,7 +314,7 @@ Generate 3D ray directions from corrected (undistorted) image coordinates.
|
|
|
314
314
|
Get the camera position (key-point) in world space.
|
|
315
315
|
|
|
316
316
|
**Returns:**
|
|
317
|
-
- `np.ndarray`: Shape (3,
|
|
317
|
+
- `np.ndarray`: Shape (3,) array with [x, y, z] camera position
|
|
318
318
|
|
|
319
319
|
## Calibration File Format
|
|
320
320
|
|
|
@@ -148,7 +148,7 @@ rays = camera.ctd_to_ray(ctd_points)
|
|
|
148
148
|
|
|
149
149
|
# Get camera position in world space (ray origin)
|
|
150
150
|
key_point = camera.get_key_point()
|
|
151
|
-
print(key_point.shape) # (3,
|
|
151
|
+
print(key_point.shape) # (3,) - [x, y, z] camera position
|
|
152
152
|
|
|
153
153
|
# Ray equation: point_on_ray = key_point + t * ray_direction
|
|
154
154
|
# All rays are normalized to unit length
|
|
@@ -284,7 +284,7 @@ Generate 3D ray directions from corrected (undistorted) image coordinates.
|
|
|
284
284
|
Get the camera position (key-point) in world space.
|
|
285
285
|
|
|
286
286
|
**Returns:**
|
|
287
|
-
- `np.ndarray`: Shape (3,
|
|
287
|
+
- `np.ndarray`: Shape (3,) array with [x, y, z] camera position
|
|
288
288
|
|
|
289
289
|
## Calibration File Format
|
|
290
290
|
|
|
@@ -0,0 +1,303 @@
|
|
|
1
|
+
import numpy as np
|
|
2
|
+
import sympy as sp
|
|
3
|
+
from camera_client.loading import read_npz_file
|
|
4
|
+
|
|
5
|
+
|
|
6
|
+
class CameraProjection:
|
|
7
|
+
"""
|
|
8
|
+
Vectorized camera coordinate transformer for batch processing.
|
|
9
|
+
|
|
10
|
+
This class is optimized for processing multiple points at once.
|
|
11
|
+
All methods expect array inputs with shape (N, 2) or (N, 3).
|
|
12
|
+
|
|
13
|
+
Handles transformations between:
|
|
14
|
+
- src: Source (distorted) image coordinates
|
|
15
|
+
- ctd: Corrected (undistorted) image coordinates
|
|
16
|
+
- gnd: Ground (3D world) coordinates
|
|
17
|
+
"""
|
|
18
|
+
|
|
19
|
+
def __init__(self, cam_archive_data):
|
|
20
|
+
"""
|
|
21
|
+
Initialize the camera transformer with calibration data.
|
|
22
|
+
|
|
23
|
+
Args:
|
|
24
|
+
cam_archive_data: Dictionary containing:
|
|
25
|
+
- src2ctd: Source to corrected distortion map (H x W x 2)
|
|
26
|
+
- ctd2src: Corrected to source distortion map (H x W x 2)
|
|
27
|
+
- x_gnd, y_gnd, z_gnd: String expressions for ctd -> ground
|
|
28
|
+
- x_im, y_im: String expressions for ground -> ctd
|
|
29
|
+
"""
|
|
30
|
+
data = cam_archive_data
|
|
31
|
+
|
|
32
|
+
self.plan_scale = float(data["plan_scale"])
|
|
33
|
+
self.im_width = int(data["im_width"])
|
|
34
|
+
self.im_height = int(data["im_height"])
|
|
35
|
+
self.im_wh_size = (self.im_width, self.im_height)
|
|
36
|
+
|
|
37
|
+
# Store lookup tables
|
|
38
|
+
self.src2ctd_points_map = data["src2ctd"]
|
|
39
|
+
self.ctd2src_points_map = data["ctd2src"]
|
|
40
|
+
|
|
41
|
+
self.im_size = self.src2ctd_points_map.shape[:2]
|
|
42
|
+
|
|
43
|
+
# Compile transformation expressions for ctd -> gnd
|
|
44
|
+
x_im, y_im, proj_height = sp.symbols("x_im y_im proj_height")
|
|
45
|
+
self._lambda_im2gnd = sp.lambdify(
|
|
46
|
+
(x_im, y_im, proj_height), sp.sympify(data["exp_im2gnd"]), "numpy"
|
|
47
|
+
)
|
|
48
|
+
|
|
49
|
+
# Compile transformation expressions for gnd -> ctd
|
|
50
|
+
x_gnd, y_gnd, z_gnd = sp.symbols("x_gnd y_gnd z_gnd")
|
|
51
|
+
self._lambda_gnd2im = sp.lambdify(
|
|
52
|
+
(x_gnd, y_gnd, z_gnd), sp.sympify(data["exp_gnd2im"]), "numpy"
|
|
53
|
+
)
|
|
54
|
+
|
|
55
|
+
# Compile keypoint expressions (camera position in world space)
|
|
56
|
+
self._lambda_key_point = sp.lambdify(
|
|
57
|
+
(), sp.sympify(data["exp_key_point"]), "numpy"
|
|
58
|
+
)
|
|
59
|
+
|
|
60
|
+
# Compile ray direction expressions for ctd -> ray
|
|
61
|
+
self._lambda_im2ray = sp.lambdify(
|
|
62
|
+
(x_im, y_im), sp.sympify(data["exp_im2ray"]), "numpy"
|
|
63
|
+
)
|
|
64
|
+
|
|
65
|
+
def src_to_ctd(self, points):
|
|
66
|
+
"""
|
|
67
|
+
Transform from source (distorted) to corrected coordinates.
|
|
68
|
+
|
|
69
|
+
Args:
|
|
70
|
+
points: (N, 2) array of source points [[x1, y1], [x2, y2], ...]
|
|
71
|
+
|
|
72
|
+
Returns:
|
|
73
|
+
(N, 2) array of corrected points, with [nan, nan] for out-of-bounds
|
|
74
|
+
"""
|
|
75
|
+
points = np.asarray(points, dtype=float)
|
|
76
|
+
if points.ndim != 2 or points.shape[1] != 2:
|
|
77
|
+
raise ValueError(f"Expected (N, 2) array, got shape {points.shape}")
|
|
78
|
+
|
|
79
|
+
N = len(points)
|
|
80
|
+
result = np.full((N, 2), np.nan, dtype=float)
|
|
81
|
+
|
|
82
|
+
# Check for NaN input points
|
|
83
|
+
valid_input = ~np.isnan(points).any(axis=1)
|
|
84
|
+
|
|
85
|
+
if not valid_input.any():
|
|
86
|
+
return result
|
|
87
|
+
|
|
88
|
+
# Round to integer coordinates
|
|
89
|
+
p_int = np.round(points[valid_input]).astype(int)
|
|
90
|
+
|
|
91
|
+
# Vectorized bounds checking
|
|
92
|
+
in_bounds = (
|
|
93
|
+
(p_int[:, 0] >= 0)
|
|
94
|
+
& (p_int[:, 0] < self.src2ctd_points_map.shape[1])
|
|
95
|
+
& (p_int[:, 1] >= 0)
|
|
96
|
+
& (p_int[:, 1] < self.src2ctd_points_map.shape[0])
|
|
97
|
+
)
|
|
98
|
+
|
|
99
|
+
# Create mask for points that are both valid input and in bounds
|
|
100
|
+
valid_indices = np.where(valid_input)[0]
|
|
101
|
+
final_valid_indices = valid_indices[in_bounds]
|
|
102
|
+
valid_p_int = p_int[in_bounds]
|
|
103
|
+
|
|
104
|
+
# Vectorized lookup using advanced indexing
|
|
105
|
+
# Note: lookup map is [y, x] indexed
|
|
106
|
+
result[final_valid_indices] = self.src2ctd_points_map[
|
|
107
|
+
valid_p_int[:, 1], valid_p_int[:, 0]
|
|
108
|
+
]
|
|
109
|
+
|
|
110
|
+
return result
|
|
111
|
+
|
|
112
|
+
def ctd_to_src(self, points):
|
|
113
|
+
"""
|
|
114
|
+
Transform from corrected to source (distorted) coordinates.
|
|
115
|
+
|
|
116
|
+
Args:
|
|
117
|
+
points: (N, 2) array of corrected points [[x1, y1], [x2, y2], ...]
|
|
118
|
+
|
|
119
|
+
Returns:
|
|
120
|
+
(N, 2) array of source points, with [nan, nan] for out-of-bounds
|
|
121
|
+
"""
|
|
122
|
+
points = np.asarray(points, dtype=float)
|
|
123
|
+
if points.ndim != 2 or points.shape[1] != 2:
|
|
124
|
+
raise ValueError(f"Expected (N, 2) array, got shape {points.shape}")
|
|
125
|
+
|
|
126
|
+
N = len(points)
|
|
127
|
+
result = np.full((N, 2), np.nan, dtype=float)
|
|
128
|
+
|
|
129
|
+
# Check for NaN input points
|
|
130
|
+
valid_input = ~np.isnan(points).any(axis=1)
|
|
131
|
+
|
|
132
|
+
if not valid_input.any():
|
|
133
|
+
return result
|
|
134
|
+
|
|
135
|
+
# Round to integer coordinates
|
|
136
|
+
p_int = np.round(points[valid_input]).astype(int)
|
|
137
|
+
|
|
138
|
+
# Vectorized bounds checking
|
|
139
|
+
in_bounds = (
|
|
140
|
+
(p_int[:, 0] >= 0)
|
|
141
|
+
& (p_int[:, 0] < self.ctd2src_points_map.shape[1])
|
|
142
|
+
& (p_int[:, 1] >= 0)
|
|
143
|
+
& (p_int[:, 1] < self.ctd2src_points_map.shape[0])
|
|
144
|
+
)
|
|
145
|
+
|
|
146
|
+
# Create mask for points that are both valid input and in bounds
|
|
147
|
+
valid_indices = np.where(valid_input)[0]
|
|
148
|
+
final_valid_indices = valid_indices[in_bounds]
|
|
149
|
+
valid_p_int = p_int[in_bounds]
|
|
150
|
+
|
|
151
|
+
# Vectorized lookup using advanced indexing
|
|
152
|
+
result[final_valid_indices] = self.ctd2src_points_map[
|
|
153
|
+
valid_p_int[:, 1], valid_p_int[:, 0]
|
|
154
|
+
]
|
|
155
|
+
|
|
156
|
+
return result
|
|
157
|
+
|
|
158
|
+
def ctd_to_gnd(self, points, h):
|
|
159
|
+
"""
|
|
160
|
+
Transform from corrected image coordinates to ground (3D world) coordinates.
|
|
161
|
+
|
|
162
|
+
Args:
|
|
163
|
+
points: (N, 2) array of corrected points [[x1, y1], [x2, y2], ...]
|
|
164
|
+
h: Scalar height or (N,) array of heights for each point
|
|
165
|
+
|
|
166
|
+
Returns:
|
|
167
|
+
(N, 3) array of ground points [[x1, y1, z1], ...]
|
|
168
|
+
"""
|
|
169
|
+
points = np.asarray(points, dtype=float)
|
|
170
|
+
if points.ndim != 2 or points.shape[1] != 2:
|
|
171
|
+
raise ValueError(f"Expected (N, 2) array, got shape {points.shape}")
|
|
172
|
+
|
|
173
|
+
N = points.shape[0]
|
|
174
|
+
x_im, y_im = points.T
|
|
175
|
+
hs = np.asarray(h, dtype=float)
|
|
176
|
+
|
|
177
|
+
# Call lambdified function: returns shape (3, 1, N)
|
|
178
|
+
# Reshape directly to (N, 3)
|
|
179
|
+
ps_gnd = np.asarray(self._lambda_im2gnd(x_im, y_im, hs)).reshape(3, N).T
|
|
180
|
+
|
|
181
|
+
return ps_gnd
|
|
182
|
+
|
|
183
|
+
def gnd_to_ctd(self, points):
|
|
184
|
+
"""
|
|
185
|
+
Transform from ground (3D world) to corrected image coordinates.
|
|
186
|
+
|
|
187
|
+
Args:
|
|
188
|
+
points: (N, 3) array of ground points [[x1, y1, z1], [x2, y2, z2], ...]
|
|
189
|
+
|
|
190
|
+
Returns:
|
|
191
|
+
(N, 2) array of image points [[x1, y1], ...]
|
|
192
|
+
"""
|
|
193
|
+
points = np.asarray(points, dtype=float)
|
|
194
|
+
if points.ndim != 2 or points.shape[1] != 3:
|
|
195
|
+
raise ValueError(f"Expected (N, 3) array, got shape {points.shape}")
|
|
196
|
+
|
|
197
|
+
N = points.shape[0]
|
|
198
|
+
x_gnd, y_gnd, z_gnd = points.T
|
|
199
|
+
|
|
200
|
+
# Call lambdified function: returns shape (2, 1, N)
|
|
201
|
+
# Reshape directly to (N, 2)
|
|
202
|
+
ps_im = np.asarray(self._lambda_gnd2im(x_gnd, y_gnd, z_gnd)).reshape(2, N).T
|
|
203
|
+
|
|
204
|
+
return ps_im
|
|
205
|
+
|
|
206
|
+
def src_to_gnd(self, points, h):
|
|
207
|
+
"""
|
|
208
|
+
Transform from source to ground coordinates.
|
|
209
|
+
|
|
210
|
+
Args:
|
|
211
|
+
points: (N, 2) array of source points
|
|
212
|
+
h: Scalar height or (N,) array of heights
|
|
213
|
+
|
|
214
|
+
Returns:
|
|
215
|
+
(N, 3) array of ground points
|
|
216
|
+
"""
|
|
217
|
+
ctd_points = self.src_to_ctd(points)
|
|
218
|
+
return self.ctd_to_gnd(ctd_points, h)
|
|
219
|
+
|
|
220
|
+
def gnd_to_src(self, points):
|
|
221
|
+
"""
|
|
222
|
+
Transform from ground to source coordinates.
|
|
223
|
+
|
|
224
|
+
Args:
|
|
225
|
+
points: (N, 3) array of ground points
|
|
226
|
+
|
|
227
|
+
Returns:
|
|
228
|
+
(N, 2) array of source points
|
|
229
|
+
"""
|
|
230
|
+
ctd_points = self.gnd_to_ctd(points)
|
|
231
|
+
return self.ctd_to_src(ctd_points)
|
|
232
|
+
|
|
233
|
+
def get_key_point(self):
|
|
234
|
+
"""
|
|
235
|
+
Get the key-point of the camera in world space.
|
|
236
|
+
|
|
237
|
+
Returns:
|
|
238
|
+
(3,) 3D coordinates of the key-point of the camera in world space
|
|
239
|
+
"""
|
|
240
|
+
key_point = self._lambda_key_point()
|
|
241
|
+
return np.array(key_point, dtype=np.float64).flatten()
|
|
242
|
+
|
|
243
|
+
def ctd_to_ray(self, points):
|
|
244
|
+
"""
|
|
245
|
+
Transform from corrected image coordinates to 3D rays in camera space.
|
|
246
|
+
|
|
247
|
+
Args:
|
|
248
|
+
points: (N, 2) array of corrected image points [[x1, y1], [x2, y2], ...]
|
|
249
|
+
|
|
250
|
+
Returns:
|
|
251
|
+
(N, 3) array of ray directions in camera space, with [nan, nan, nan] for invalid
|
|
252
|
+
Ray directions are normalized to unit length.
|
|
253
|
+
The ray for a point is defined as the vector from key-point of the camera.
|
|
254
|
+
"""
|
|
255
|
+
points = np.asarray(points, dtype=float)
|
|
256
|
+
if points.ndim != 2 or points.shape[1] != 2:
|
|
257
|
+
raise ValueError(f"Expected (N, 2) array, got shape {points.shape}")
|
|
258
|
+
|
|
259
|
+
N = points.shape[0]
|
|
260
|
+
x_im, y_im = points.T
|
|
261
|
+
|
|
262
|
+
# Call lambdified function: returns shape (3, 1, N)
|
|
263
|
+
# Reshape directly to (N, 3)
|
|
264
|
+
rays = np.asarray(self._lambda_im2ray(x_im, y_im)).reshape(3, N).T
|
|
265
|
+
|
|
266
|
+
# Normalize ray directions to unit length
|
|
267
|
+
ray_lengths = np.linalg.norm(rays, axis=1, keepdims=True)
|
|
268
|
+
ray_lengths = np.where(ray_lengths > 0, ray_lengths, 1.0)
|
|
269
|
+
rays_normalized = rays / ray_lengths
|
|
270
|
+
|
|
271
|
+
return rays_normalized
|
|
272
|
+
|
|
273
|
+
def src_to_ray(self, points):
|
|
274
|
+
"""
|
|
275
|
+
Transform from source image coordinates to 3D rays in camera space.
|
|
276
|
+
|
|
277
|
+
Args:
|
|
278
|
+
points: (N, 2) array of source points [[x1, y1], [x2, y2], ...]
|
|
279
|
+
|
|
280
|
+
Returns:
|
|
281
|
+
(N, 3) array of ray directions in camera space, with [nan, nan, nan] for invalid
|
|
282
|
+
Ray directions are normalized to unit length.
|
|
283
|
+
The ray for a point is defined as the vector from key-point of the camera.
|
|
284
|
+
"""
|
|
285
|
+
ctd_points = self.src_to_ctd(points)
|
|
286
|
+
return self.ctd_to_ray(ctd_points)
|
|
287
|
+
|
|
288
|
+
@classmethod
|
|
289
|
+
def load(cls, archive_path):
|
|
290
|
+
"""
|
|
291
|
+
Load camera transformer from NPZ archive file.
|
|
292
|
+
|
|
293
|
+
Args:
|
|
294
|
+
archive_path: Path to .npz calibration file
|
|
295
|
+
|
|
296
|
+
Returns:
|
|
297
|
+
CameraProjectionVectorized instance
|
|
298
|
+
"""
|
|
299
|
+
|
|
300
|
+
# Convert to regular dict for easier access
|
|
301
|
+
cam_data = read_npz_file(archive_path)
|
|
302
|
+
|
|
303
|
+
return cls(cam_data)
|
|
@@ -1,6 +1,6 @@
|
|
|
1
1
|
Metadata-Version: 2.4
|
|
2
2
|
Name: camera-client
|
|
3
|
-
Version: 0.2.
|
|
3
|
+
Version: 0.2.1
|
|
4
4
|
Summary: Python SDK for camera calibration and projection transformations - handle lens distortion, coordinate transformations, and 3D ray casting with symbolic expressions.
|
|
5
5
|
Author-email: Alexander Abramov <extremal.ru@gmail.com>
|
|
6
6
|
License: MIT
|
|
@@ -178,7 +178,7 @@ rays = camera.ctd_to_ray(ctd_points)
|
|
|
178
178
|
|
|
179
179
|
# Get camera position in world space (ray origin)
|
|
180
180
|
key_point = camera.get_key_point()
|
|
181
|
-
print(key_point.shape) # (3,
|
|
181
|
+
print(key_point.shape) # (3,) - [x, y, z] camera position
|
|
182
182
|
|
|
183
183
|
# Ray equation: point_on_ray = key_point + t * ray_direction
|
|
184
184
|
# All rays are normalized to unit length
|
|
@@ -314,7 +314,7 @@ Generate 3D ray directions from corrected (undistorted) image coordinates.
|
|
|
314
314
|
Get the camera position (key-point) in world space.
|
|
315
315
|
|
|
316
316
|
**Returns:**
|
|
317
|
-
- `np.ndarray`: Shape (3,
|
|
317
|
+
- `np.ndarray`: Shape (3,) array with [x, y, z] camera position
|
|
318
318
|
|
|
319
319
|
## Calibration File Format
|
|
320
320
|
|
|
@@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta"
|
|
|
4
4
|
|
|
5
5
|
[project]
|
|
6
6
|
name = "camera-client"
|
|
7
|
-
version = "0.2.
|
|
7
|
+
version = "0.2.1"
|
|
8
8
|
description = "Python SDK for camera calibration and projection transformations - handle lens distortion, coordinate transformations, and 3D ray casting with symbolic expressions."
|
|
9
9
|
readme = "README.md"
|
|
10
10
|
requires-python = ">=3.7"
|
|
@@ -1,724 +0,0 @@
|
|
|
1
|
-
import numpy as np
|
|
2
|
-
import sympy as sp
|
|
3
|
-
from camera_client.loading import read_npz_file
|
|
4
|
-
|
|
5
|
-
|
|
6
|
-
class CameraProjection:
|
|
7
|
-
"""
|
|
8
|
-
Vectorized camera coordinate transformer for batch processing.
|
|
9
|
-
|
|
10
|
-
This class is optimized for processing multiple points at once.
|
|
11
|
-
All methods expect array inputs with shape (N, 2) or (N, 3).
|
|
12
|
-
|
|
13
|
-
Handles transformations between:
|
|
14
|
-
- src: Source (distorted) image coordinates
|
|
15
|
-
- ctd: Corrected (undistorted) image coordinates
|
|
16
|
-
- gnd: Ground (3D world) coordinates
|
|
17
|
-
"""
|
|
18
|
-
|
|
19
|
-
def __init__(self, cam_archive_data):
|
|
20
|
-
"""
|
|
21
|
-
Initialize the camera transformer with calibration data.
|
|
22
|
-
|
|
23
|
-
Args:
|
|
24
|
-
cam_archive_data: Dictionary containing:
|
|
25
|
-
- src2ctd: Source to corrected distortion map (H x W x 2)
|
|
26
|
-
- ctd2src: Corrected to source distortion map (H x W x 2)
|
|
27
|
-
- x_gnd, y_gnd, z_gnd: String expressions for ctd -> ground
|
|
28
|
-
- x_im, y_im: String expressions for ground -> ctd
|
|
29
|
-
"""
|
|
30
|
-
data = cam_archive_data
|
|
31
|
-
|
|
32
|
-
self.plan_scale = float(data["plan_scale"])
|
|
33
|
-
self.im_width = int(data["im_width"])
|
|
34
|
-
self.im_height = int(data["im_height"])
|
|
35
|
-
self.im_wh_size = (self.im_width, self.im_height)
|
|
36
|
-
|
|
37
|
-
# Store lookup tables
|
|
38
|
-
self.src2ctd_points_map = data["src2ctd"]
|
|
39
|
-
self.ctd2src_points_map = data["ctd2src"]
|
|
40
|
-
|
|
41
|
-
self.im_size = self.src2ctd_points_map.shape[:2]
|
|
42
|
-
|
|
43
|
-
# Compile transformation expressions for ctd -> gnd
|
|
44
|
-
x_im, y_im, proj_height = sp.symbols("x_im y_im proj_height")
|
|
45
|
-
self._lambda_im2gnd = sp.lambdify(
|
|
46
|
-
(x_im, y_im, proj_height), sp.sympify(data["exp_im2gnd"]), "numpy"
|
|
47
|
-
)
|
|
48
|
-
|
|
49
|
-
# Compile transformation expressions for gnd -> ctd
|
|
50
|
-
x_gnd, y_gnd, z_gnd = sp.symbols("x_gnd y_gnd z_gnd")
|
|
51
|
-
self._lambda_gnd2im = sp.lambdify(
|
|
52
|
-
(x_gnd, y_gnd, z_gnd), sp.sympify(data["exp_gnd2im"]), "numpy"
|
|
53
|
-
)
|
|
54
|
-
|
|
55
|
-
# Compile keypoint expressions (camera position in world space)
|
|
56
|
-
self._lambda_key_point = sp.lambdify(
|
|
57
|
-
(), sp.sympify(data["exp_key_point"]), "numpy"
|
|
58
|
-
)
|
|
59
|
-
|
|
60
|
-
# Compile ray direction expressions for ctd -> ray
|
|
61
|
-
self._lambda_im2ray = sp.lambdify(
|
|
62
|
-
(x_im, y_im), sp.sympify(data["exp_im2ray"]), "numpy"
|
|
63
|
-
)
|
|
64
|
-
|
|
65
|
-
def src_to_ctd(self, points):
|
|
66
|
-
"""
|
|
67
|
-
Transform from source (distorted) to corrected coordinates.
|
|
68
|
-
|
|
69
|
-
Args:
|
|
70
|
-
points: (N, 2) array of source points [[x1, y1], [x2, y2], ...]
|
|
71
|
-
|
|
72
|
-
Returns:
|
|
73
|
-
(N, 2) array of corrected points, with [nan, nan] for out-of-bounds
|
|
74
|
-
"""
|
|
75
|
-
points = np.asarray(points, dtype=float)
|
|
76
|
-
if points.ndim != 2 or points.shape[1] != 2:
|
|
77
|
-
raise ValueError(f"Expected (N, 2) array, got shape {points.shape}")
|
|
78
|
-
|
|
79
|
-
N = len(points)
|
|
80
|
-
result = np.full((N, 2), np.nan, dtype=float)
|
|
81
|
-
|
|
82
|
-
# Check for NaN input points
|
|
83
|
-
valid_input = ~np.isnan(points).any(axis=1)
|
|
84
|
-
|
|
85
|
-
if not valid_input.any():
|
|
86
|
-
return result
|
|
87
|
-
|
|
88
|
-
# Round to integer coordinates
|
|
89
|
-
p_int = np.round(points[valid_input]).astype(int)
|
|
90
|
-
|
|
91
|
-
# Vectorized bounds checking
|
|
92
|
-
in_bounds = (
|
|
93
|
-
(p_int[:, 0] >= 0)
|
|
94
|
-
& (p_int[:, 0] < self.src2ctd_points_map.shape[1])
|
|
95
|
-
& (p_int[:, 1] >= 0)
|
|
96
|
-
& (p_int[:, 1] < self.src2ctd_points_map.shape[0])
|
|
97
|
-
)
|
|
98
|
-
|
|
99
|
-
# Create mask for points that are both valid input and in bounds
|
|
100
|
-
valid_indices = np.where(valid_input)[0]
|
|
101
|
-
final_valid_indices = valid_indices[in_bounds]
|
|
102
|
-
valid_p_int = p_int[in_bounds]
|
|
103
|
-
|
|
104
|
-
# Vectorized lookup using advanced indexing
|
|
105
|
-
# Note: lookup map is [y, x] indexed
|
|
106
|
-
result[final_valid_indices] = self.src2ctd_points_map[
|
|
107
|
-
valid_p_int[:, 1], valid_p_int[:, 0]
|
|
108
|
-
]
|
|
109
|
-
|
|
110
|
-
return result
|
|
111
|
-
|
|
112
|
-
def ctd_to_src(self, points):
|
|
113
|
-
"""
|
|
114
|
-
Transform from corrected to source (distorted) coordinates.
|
|
115
|
-
|
|
116
|
-
Args:
|
|
117
|
-
points: (N, 2) array of corrected points [[x1, y1], [x2, y2], ...]
|
|
118
|
-
|
|
119
|
-
Returns:
|
|
120
|
-
(N, 2) array of source points, with [nan, nan] for out-of-bounds
|
|
121
|
-
"""
|
|
122
|
-
points = np.asarray(points, dtype=float)
|
|
123
|
-
if points.ndim != 2 or points.shape[1] != 2:
|
|
124
|
-
raise ValueError(f"Expected (N, 2) array, got shape {points.shape}")
|
|
125
|
-
|
|
126
|
-
N = len(points)
|
|
127
|
-
result = np.full((N, 2), np.nan, dtype=float)
|
|
128
|
-
|
|
129
|
-
# Check for NaN input points
|
|
130
|
-
valid_input = ~np.isnan(points).any(axis=1)
|
|
131
|
-
|
|
132
|
-
if not valid_input.any():
|
|
133
|
-
return result
|
|
134
|
-
|
|
135
|
-
# Round to integer coordinates
|
|
136
|
-
p_int = np.round(points[valid_input]).astype(int)
|
|
137
|
-
|
|
138
|
-
# Vectorized bounds checking
|
|
139
|
-
in_bounds = (
|
|
140
|
-
(p_int[:, 0] >= 0)
|
|
141
|
-
& (p_int[:, 0] < self.ctd2src_points_map.shape[1])
|
|
142
|
-
& (p_int[:, 1] >= 0)
|
|
143
|
-
& (p_int[:, 1] < self.ctd2src_points_map.shape[0])
|
|
144
|
-
)
|
|
145
|
-
|
|
146
|
-
# Create mask for points that are both valid input and in bounds
|
|
147
|
-
valid_indices = np.where(valid_input)[0]
|
|
148
|
-
final_valid_indices = valid_indices[in_bounds]
|
|
149
|
-
valid_p_int = p_int[in_bounds]
|
|
150
|
-
|
|
151
|
-
# Vectorized lookup using advanced indexing
|
|
152
|
-
result[final_valid_indices] = self.ctd2src_points_map[
|
|
153
|
-
valid_p_int[:, 1], valid_p_int[:, 0]
|
|
154
|
-
]
|
|
155
|
-
|
|
156
|
-
return result
|
|
157
|
-
|
|
158
|
-
def ctd_to_gnd(self, points, h):
|
|
159
|
-
"""
|
|
160
|
-
Transform from corrected image coordinates to ground (3D world) coordinates.
|
|
161
|
-
|
|
162
|
-
Args:
|
|
163
|
-
points: (N, 2) array of corrected points [[x1, y1], [x2, y2], ...]
|
|
164
|
-
h: Scalar height or (N,) array of heights for each point
|
|
165
|
-
|
|
166
|
-
Returns:
|
|
167
|
-
(N, 3) array of ground points [[x1, y1, z1], ...]
|
|
168
|
-
"""
|
|
169
|
-
points = np.asarray(points, dtype=float)
|
|
170
|
-
if points.ndim != 2 or points.shape[1] != 2:
|
|
171
|
-
raise ValueError(f"Expected (N, 2) array, got shape {points.shape}")
|
|
172
|
-
|
|
173
|
-
N = points.shape[0]
|
|
174
|
-
x_im, y_im = points.T
|
|
175
|
-
hs = np.asarray(h, dtype=float)
|
|
176
|
-
|
|
177
|
-
# Call lambdified function: returns shape (3, 1, N)
|
|
178
|
-
# Reshape directly to (N, 3)
|
|
179
|
-
ps_gnd = np.asarray(self._lambda_im2gnd(x_im, y_im, hs)).reshape(3, N).T
|
|
180
|
-
|
|
181
|
-
return ps_gnd
|
|
182
|
-
|
|
183
|
-
def gnd_to_ctd(self, points):
|
|
184
|
-
"""
|
|
185
|
-
Transform from ground (3D world) to corrected image coordinates.
|
|
186
|
-
|
|
187
|
-
Args:
|
|
188
|
-
points: (N, 3) array of ground points [[x1, y1, z1], [x2, y2, z2], ...]
|
|
189
|
-
|
|
190
|
-
Returns:
|
|
191
|
-
(N, 2) array of image points [[x1, y1], ...]
|
|
192
|
-
"""
|
|
193
|
-
points = np.asarray(points, dtype=float)
|
|
194
|
-
if points.ndim != 2 or points.shape[1] != 3:
|
|
195
|
-
raise ValueError(f"Expected (N, 3) array, got shape {points.shape}")
|
|
196
|
-
|
|
197
|
-
N = points.shape[0]
|
|
198
|
-
x_gnd, y_gnd, z_gnd = points.T
|
|
199
|
-
|
|
200
|
-
# Call lambdified function: returns shape (2, 1, N)
|
|
201
|
-
# Reshape directly to (N, 2)
|
|
202
|
-
ps_im = np.asarray(self._lambda_gnd2im(x_gnd, y_gnd, z_gnd)).reshape(2, N).T
|
|
203
|
-
|
|
204
|
-
return ps_im
|
|
205
|
-
|
|
206
|
-
def src_to_gnd(self, points, h):
|
|
207
|
-
"""
|
|
208
|
-
Transform from source to ground coordinates.
|
|
209
|
-
|
|
210
|
-
Args:
|
|
211
|
-
points: (N, 2) array of source points
|
|
212
|
-
h: Scalar height or (N,) array of heights
|
|
213
|
-
|
|
214
|
-
Returns:
|
|
215
|
-
(N, 3) array of ground points
|
|
216
|
-
"""
|
|
217
|
-
ctd_points = self.src_to_ctd(points)
|
|
218
|
-
return self.ctd_to_gnd(ctd_points, h)
|
|
219
|
-
|
|
220
|
-
def gnd_to_src(self, points):
|
|
221
|
-
"""
|
|
222
|
-
Transform from ground to source coordinates.
|
|
223
|
-
|
|
224
|
-
Args:
|
|
225
|
-
points: (N, 3) array of ground points
|
|
226
|
-
|
|
227
|
-
Returns:
|
|
228
|
-
(N, 2) array of source points
|
|
229
|
-
"""
|
|
230
|
-
ctd_points = self.gnd_to_ctd(points)
|
|
231
|
-
return self.ctd_to_src(ctd_points)
|
|
232
|
-
|
|
233
|
-
def get_key_point(self):
|
|
234
|
-
"""
|
|
235
|
-
Get the key-point of the camera in world space.
|
|
236
|
-
|
|
237
|
-
Returns:
|
|
238
|
-
(3,) 3D coordinates of the key-point of the camera in world space
|
|
239
|
-
"""
|
|
240
|
-
key_point = self._lambda_key_point()
|
|
241
|
-
return np.array(key_point, dtype=np.float64)
|
|
242
|
-
|
|
243
|
-
def ctd_to_ray(self, points):
|
|
244
|
-
"""
|
|
245
|
-
Transform from corrected image coordinates to 3D rays in camera space.
|
|
246
|
-
|
|
247
|
-
Args:
|
|
248
|
-
points: (N, 2) array of corrected image points [[x1, y1], [x2, y2], ...]
|
|
249
|
-
|
|
250
|
-
Returns:
|
|
251
|
-
(N, 3) array of ray directions in camera space, with [nan, nan, nan] for invalid
|
|
252
|
-
Ray directions are normalized to unit length.
|
|
253
|
-
The ray for a point is defined as the vector from key-point of the camera.
|
|
254
|
-
"""
|
|
255
|
-
points = np.asarray(points, dtype=float)
|
|
256
|
-
if points.ndim != 2 or points.shape[1] != 2:
|
|
257
|
-
raise ValueError(f"Expected (N, 2) array, got shape {points.shape}")
|
|
258
|
-
|
|
259
|
-
N = points.shape[0]
|
|
260
|
-
x_im, y_im = points.T
|
|
261
|
-
|
|
262
|
-
# Call lambdified function: returns shape (3, 1, N)
|
|
263
|
-
# Reshape directly to (N, 3)
|
|
264
|
-
rays = np.asarray(self._lambda_im2ray(x_im, y_im)).reshape(3, N).T
|
|
265
|
-
|
|
266
|
-
# Normalize ray directions to unit length
|
|
267
|
-
ray_lengths = np.linalg.norm(rays, axis=1, keepdims=True)
|
|
268
|
-
ray_lengths = np.where(ray_lengths > 0, ray_lengths, 1.0)
|
|
269
|
-
rays_normalized = rays / ray_lengths
|
|
270
|
-
|
|
271
|
-
return rays_normalized
|
|
272
|
-
|
|
273
|
-
def src_to_ray(self, points):
|
|
274
|
-
"""
|
|
275
|
-
Transform from source image coordinates to 3D rays in camera space.
|
|
276
|
-
|
|
277
|
-
Args:
|
|
278
|
-
points: (N, 2) array of source points [[x1, y1], [x2, y2], ...]
|
|
279
|
-
|
|
280
|
-
Returns:
|
|
281
|
-
(N, 3) array of ray directions in camera space, with [nan, nan, nan] for invalid
|
|
282
|
-
Ray directions are normalized to unit length.
|
|
283
|
-
The ray for a point is defined as the vector from key-point of the camera.
|
|
284
|
-
"""
|
|
285
|
-
ctd_points = self.src_to_ctd(points)
|
|
286
|
-
return self.ctd_to_ray(ctd_points)
|
|
287
|
-
|
|
288
|
-
@classmethod
|
|
289
|
-
def load(cls, archive_path):
|
|
290
|
-
"""
|
|
291
|
-
Load camera transformer from NPZ archive file.
|
|
292
|
-
|
|
293
|
-
Args:
|
|
294
|
-
archive_path: Path to .npz calibration file
|
|
295
|
-
|
|
296
|
-
Returns:
|
|
297
|
-
CameraProjectionVectorized instance
|
|
298
|
-
"""
|
|
299
|
-
|
|
300
|
-
# Convert to regular dict for easier access
|
|
301
|
-
cam_data = read_npz_file(archive_path)
|
|
302
|
-
|
|
303
|
-
return cls(cam_data)
|
|
304
|
-
|
|
305
|
-
|
|
306
|
-
# class CameraProjection2:
|
|
307
|
-
# """
|
|
308
|
-
# Vectorized camera coordinate transformer for batch processing.
|
|
309
|
-
|
|
310
|
-
# This class is optimized for processing multiple points at once.
|
|
311
|
-
# All methods expect array inputs with shape (N, 2) or (N, 3).
|
|
312
|
-
|
|
313
|
-
# Handles transformations between:
|
|
314
|
-
# - src: Source (distorted) image coordinates
|
|
315
|
-
# - ctd: Corrected (undistorted) image coordinates
|
|
316
|
-
# - gnd: Ground (3D world) coordinates
|
|
317
|
-
# """
|
|
318
|
-
|
|
319
|
-
# # Allowed variable names for different transformation types
|
|
320
|
-
# VARS_CTD_TO_GND = {
|
|
321
|
-
# "x_im",
|
|
322
|
-
# "y_im",
|
|
323
|
-
# "proj_height",
|
|
324
|
-
# "np",
|
|
325
|
-
# "float64",
|
|
326
|
-
# "asarray",
|
|
327
|
-
# "dtype",
|
|
328
|
-
# }
|
|
329
|
-
# VARS_GND_TO_CTD = {"x_gnd", "y_gnd", "z_gnd", "np", "float64", "asarray", "dtype"}
|
|
330
|
-
# VARS_CTD_TO_RAY = {"x_im", "y_im", "np", "float64", "asarray", "dtype"}
|
|
331
|
-
# VARS_KEYPOINT = {"np", "float64", "asarray", "dtype"}
|
|
332
|
-
|
|
333
|
-
# def __init__(self, cam_archive_data):
|
|
334
|
-
# """
|
|
335
|
-
# Initialize the camera transformer with calibration data.
|
|
336
|
-
|
|
337
|
-
# Args:
|
|
338
|
-
# cam_archive_data: Dictionary containing:
|
|
339
|
-
# - src2ctd: Source to corrected distortion map (H x W x 2)
|
|
340
|
-
# - ctd2src: Corrected to source distortion map (H x W x 2)
|
|
341
|
-
# - x_gnd, y_gnd, z_gnd: String expressions for ctd -> ground
|
|
342
|
-
# - x_im, y_im: String expressions for ground -> ctd
|
|
343
|
-
# """
|
|
344
|
-
# data = cam_archive_data
|
|
345
|
-
|
|
346
|
-
# self.plan_scale = float(data["plan_scale"])
|
|
347
|
-
# self.im_width = int(data["im_width"])
|
|
348
|
-
# self.im_height = int(data["im_height"])
|
|
349
|
-
# self.im_wh_size = (self.im_width, self.im_height)
|
|
350
|
-
|
|
351
|
-
# # Store lookup tables
|
|
352
|
-
# self.src2ctd_points_map = data["src2ctd"]
|
|
353
|
-
# self.ctd2src_points_map = data["ctd2src"]
|
|
354
|
-
|
|
355
|
-
# self.im_size = self.src2ctd_points_map.shape[:2]
|
|
356
|
-
|
|
357
|
-
# # Compile transformation expressions for ctd -> gnd
|
|
358
|
-
# self._x_gnd_func = compile_safe_expression(
|
|
359
|
-
# data["x_gnd"],
|
|
360
|
-
# param_names=["x_im", "y_im", "proj_height"],
|
|
361
|
-
# allowed_vars=self.VARS_CTD_TO_GND,
|
|
362
|
-
# )
|
|
363
|
-
# self._y_gnd_func = compile_safe_expression(
|
|
364
|
-
# data["y_gnd"],
|
|
365
|
-
# param_names=["x_im", "y_im", "proj_height"],
|
|
366
|
-
# allowed_vars=self.VARS_CTD_TO_GND,
|
|
367
|
-
# )
|
|
368
|
-
# self._z_gnd_func = compile_safe_expression(
|
|
369
|
-
# data["z_gnd"],
|
|
370
|
-
# param_names=["x_im", "y_im", "proj_height"],
|
|
371
|
-
# allowed_vars=self.VARS_CTD_TO_GND,
|
|
372
|
-
# )
|
|
373
|
-
|
|
374
|
-
# # Compile transformation expressions for gnd -> ctd
|
|
375
|
-
# self._x_im_func = compile_safe_expression(
|
|
376
|
-
# data["x_im"],
|
|
377
|
-
# param_names=["x_gnd", "y_gnd", "z_gnd"],
|
|
378
|
-
# allowed_vars=self.VARS_GND_TO_CTD,
|
|
379
|
-
# )
|
|
380
|
-
# self._y_im_func = compile_safe_expression(
|
|
381
|
-
# data["y_im"],
|
|
382
|
-
# param_names=["x_gnd", "y_gnd", "z_gnd"],
|
|
383
|
-
# allowed_vars=self.VARS_GND_TO_CTD,
|
|
384
|
-
# )
|
|
385
|
-
|
|
386
|
-
# # Compile keypoint expressions (camera position in world space)
|
|
387
|
-
# self._x_key_func = compile_safe_expression(
|
|
388
|
-
# data["x_key"],
|
|
389
|
-
# param_names=[],
|
|
390
|
-
# allowed_vars=self.VARS_KEYPOINT,
|
|
391
|
-
# )
|
|
392
|
-
# self._y_key_func = compile_safe_expression(
|
|
393
|
-
# data["y_key"],
|
|
394
|
-
# param_names=[],
|
|
395
|
-
# allowed_vars=self.VARS_KEYPOINT,
|
|
396
|
-
# )
|
|
397
|
-
# self._z_key_func = compile_safe_expression(
|
|
398
|
-
# data["z_key"],
|
|
399
|
-
# param_names=[],
|
|
400
|
-
# allowed_vars=self.VARS_KEYPOINT,
|
|
401
|
-
# )
|
|
402
|
-
|
|
403
|
-
# # Compile ray direction expressions for ctd -> ray
|
|
404
|
-
# self._x_ray_func = compile_safe_expression(
|
|
405
|
-
# data["x_ray"],
|
|
406
|
-
# param_names=["x_im", "y_im"],
|
|
407
|
-
# allowed_vars=self.VARS_CTD_TO_RAY,
|
|
408
|
-
# )
|
|
409
|
-
# self._y_ray_func = compile_safe_expression(
|
|
410
|
-
# data["y_ray"],
|
|
411
|
-
# param_names=["x_im", "y_im"],
|
|
412
|
-
# allowed_vars=self.VARS_CTD_TO_RAY,
|
|
413
|
-
# )
|
|
414
|
-
# self._z_ray_func = compile_safe_expression(
|
|
415
|
-
# data["z_ray"],
|
|
416
|
-
# param_names=["x_im", "y_im"],
|
|
417
|
-
# allowed_vars=self.VARS_CTD_TO_RAY,
|
|
418
|
-
# )
|
|
419
|
-
|
|
420
|
-
# def src_to_ctd(self, points):
|
|
421
|
-
# """
|
|
422
|
-
# Transform from source (distorted) to corrected coordinates.
|
|
423
|
-
|
|
424
|
-
# Args:
|
|
425
|
-
# points: (N, 2) array of source points [[x1, y1], [x2, y2], ...]
|
|
426
|
-
|
|
427
|
-
# Returns:
|
|
428
|
-
# (N, 2) array of corrected points, with [nan, nan] for out-of-bounds
|
|
429
|
-
# """
|
|
430
|
-
# points = np.asarray(points, dtype=float)
|
|
431
|
-
# if points.ndim != 2 or points.shape[1] != 2:
|
|
432
|
-
# raise ValueError(f"Expected (N, 2) array, got shape {points.shape}")
|
|
433
|
-
|
|
434
|
-
# N = len(points)
|
|
435
|
-
# result = np.full((N, 2), np.nan, dtype=float)
|
|
436
|
-
|
|
437
|
-
# # Check for NaN input points
|
|
438
|
-
# valid_input = ~np.isnan(points).any(axis=1)
|
|
439
|
-
|
|
440
|
-
# if not valid_input.any():
|
|
441
|
-
# return result
|
|
442
|
-
|
|
443
|
-
# # Round to integer coordinates
|
|
444
|
-
# p_int = np.round(points[valid_input]).astype(int)
|
|
445
|
-
|
|
446
|
-
# # Vectorized bounds checking
|
|
447
|
-
# in_bounds = (
|
|
448
|
-
# (p_int[:, 0] >= 0)
|
|
449
|
-
# & (p_int[:, 0] < self.src2ctd_points_map.shape[1])
|
|
450
|
-
# & (p_int[:, 1] >= 0)
|
|
451
|
-
# & (p_int[:, 1] < self.src2ctd_points_map.shape[0])
|
|
452
|
-
# )
|
|
453
|
-
|
|
454
|
-
# # Create mask for points that are both valid input and in bounds
|
|
455
|
-
# valid_indices = np.where(valid_input)[0]
|
|
456
|
-
# final_valid_indices = valid_indices[in_bounds]
|
|
457
|
-
# valid_p_int = p_int[in_bounds]
|
|
458
|
-
|
|
459
|
-
# # Vectorized lookup using advanced indexing
|
|
460
|
-
# # Note: lookup map is [y, x] indexed
|
|
461
|
-
# result[final_valid_indices] = self.src2ctd_points_map[
|
|
462
|
-
# valid_p_int[:, 1], valid_p_int[:, 0]
|
|
463
|
-
# ]
|
|
464
|
-
|
|
465
|
-
# return result
|
|
466
|
-
|
|
467
|
-
# def ctd_to_src(self, points):
|
|
468
|
-
# """
|
|
469
|
-
# Transform from corrected to source (distorted) coordinates.
|
|
470
|
-
|
|
471
|
-
# Args:
|
|
472
|
-
# points: (N, 2) array of corrected points [[x1, y1], [x2, y2], ...]
|
|
473
|
-
|
|
474
|
-
# Returns:
|
|
475
|
-
# (N, 2) array of source points, with [nan, nan] for out-of-bounds
|
|
476
|
-
# """
|
|
477
|
-
# points = np.asarray(points, dtype=float)
|
|
478
|
-
# if points.ndim != 2 or points.shape[1] != 2:
|
|
479
|
-
# raise ValueError(f"Expected (N, 2) array, got shape {points.shape}")
|
|
480
|
-
|
|
481
|
-
# N = len(points)
|
|
482
|
-
# result = np.full((N, 2), np.nan, dtype=float)
|
|
483
|
-
|
|
484
|
-
# # Check for NaN input points
|
|
485
|
-
# valid_input = ~np.isnan(points).any(axis=1)
|
|
486
|
-
|
|
487
|
-
# if not valid_input.any():
|
|
488
|
-
# return result
|
|
489
|
-
|
|
490
|
-
# # Round to integer coordinates
|
|
491
|
-
# p_int = np.round(points[valid_input]).astype(int)
|
|
492
|
-
|
|
493
|
-
# # Vectorized bounds checking
|
|
494
|
-
# in_bounds = (
|
|
495
|
-
# (p_int[:, 0] >= 0)
|
|
496
|
-
# & (p_int[:, 0] < self.ctd2src_points_map.shape[1])
|
|
497
|
-
# & (p_int[:, 1] >= 0)
|
|
498
|
-
# & (p_int[:, 1] < self.ctd2src_points_map.shape[0])
|
|
499
|
-
# )
|
|
500
|
-
|
|
501
|
-
# # Create mask for points that are both valid input and in bounds
|
|
502
|
-
# valid_indices = np.where(valid_input)[0]
|
|
503
|
-
# final_valid_indices = valid_indices[in_bounds]
|
|
504
|
-
# valid_p_int = p_int[in_bounds]
|
|
505
|
-
|
|
506
|
-
# # Vectorized lookup using advanced indexing
|
|
507
|
-
# result[final_valid_indices] = self.ctd2src_points_map[
|
|
508
|
-
# valid_p_int[:, 1], valid_p_int[:, 0]
|
|
509
|
-
# ]
|
|
510
|
-
|
|
511
|
-
# return result
|
|
512
|
-
|
|
513
|
-
# def ctd_to_gnd(self, points, h):
|
|
514
|
-
# """
|
|
515
|
-
# Transform from corrected image coordinates to ground (3D world) coordinates.
|
|
516
|
-
|
|
517
|
-
# Args:
|
|
518
|
-
# points: (N, 2) array of corrected points [[x1, y1], [x2, y2], ...]
|
|
519
|
-
# h: Scalar height or (N,) array of heights for each point
|
|
520
|
-
|
|
521
|
-
# Returns:
|
|
522
|
-
# (N, 3) array of ground points [[x1, y1, z1], ...], with [nan, nan, nan] for invalid
|
|
523
|
-
# """
|
|
524
|
-
# points = np.asarray(points, dtype=float)
|
|
525
|
-
# if points.ndim != 2 or points.shape[1] != 2:
|
|
526
|
-
# raise ValueError(f"Expected (N, 2) array, got shape {points.shape}")
|
|
527
|
-
|
|
528
|
-
# N = len(points)
|
|
529
|
-
# result = np.full((N, 3), np.nan, dtype=float)
|
|
530
|
-
|
|
531
|
-
# # Handle scalar or array height
|
|
532
|
-
# if np.isscalar(h):
|
|
533
|
-
# h_array = np.full(N, h, dtype=float)
|
|
534
|
-
# else:
|
|
535
|
-
# h_array = np.asarray(h, dtype=float)
|
|
536
|
-
# if h_array.shape != (N,):
|
|
537
|
-
# raise ValueError(
|
|
538
|
-
# f"Height array must have shape ({N},), got {h_array.shape}"
|
|
539
|
-
# )
|
|
540
|
-
|
|
541
|
-
# # Check for NaN input points
|
|
542
|
-
# valid = ~np.isnan(points).any(axis=1)
|
|
543
|
-
|
|
544
|
-
# if not valid.any():
|
|
545
|
-
# return result
|
|
546
|
-
|
|
547
|
-
# # Extract valid points
|
|
548
|
-
# valid_points = points[valid]
|
|
549
|
-
# valid_heights = h_array[valid]
|
|
550
|
-
|
|
551
|
-
# # Vectorized expression evaluation
|
|
552
|
-
# # NumPy operations in expressions will work element-wise on arrays
|
|
553
|
-
# x_im = valid_points[:, 0]
|
|
554
|
-
# y_im = valid_points[:, 1]
|
|
555
|
-
# proj_height = valid_heights
|
|
556
|
-
|
|
557
|
-
# # Evaluate expressions (they should handle arrays automatically via NumPy)
|
|
558
|
-
# gnd_x = self._x_gnd_func(x_im, y_im, proj_height)
|
|
559
|
-
# gnd_y = self._y_gnd_func(x_im, y_im, proj_height)
|
|
560
|
-
# gnd_z = self._z_gnd_func(x_im, y_im, proj_height)
|
|
561
|
-
|
|
562
|
-
# # Stack results and assign to valid indices
|
|
563
|
-
# result[valid] = np.column_stack([gnd_x, gnd_y, gnd_z])
|
|
564
|
-
|
|
565
|
-
# return result
|
|
566
|
-
|
|
567
|
-
# def src_to_gnd(self, points, h):
|
|
568
|
-
# """
|
|
569
|
-
# Transform from source to ground coordinates.
|
|
570
|
-
|
|
571
|
-
# Args:
|
|
572
|
-
# points: (N, 2) array of source points
|
|
573
|
-
# h: Scalar height or (N,) array of heights
|
|
574
|
-
|
|
575
|
-
# Returns:
|
|
576
|
-
# (N, 3) array of ground points
|
|
577
|
-
# """
|
|
578
|
-
# ctd_points = self.src_to_ctd(points)
|
|
579
|
-
# return self.ctd_to_gnd(ctd_points, h)
|
|
580
|
-
|
|
581
|
-
# def gnd_to_ctd(self, points):
|
|
582
|
-
# """
|
|
583
|
-
# Transform from ground (3D world) to corrected image coordinates.
|
|
584
|
-
|
|
585
|
-
# Args:
|
|
586
|
-
# points: (N, 3) array of ground points [[x1, y1, z1], [x2, y2, z2], ...]
|
|
587
|
-
|
|
588
|
-
# Returns:
|
|
589
|
-
# (N, 2) array of image points, with [nan, nan] for invalid
|
|
590
|
-
# """
|
|
591
|
-
# points = np.asarray(points, dtype=float)
|
|
592
|
-
# if points.ndim != 2 or points.shape[1] != 3:
|
|
593
|
-
# raise ValueError(f"Expected (N, 3) array, got shape {points.shape}")
|
|
594
|
-
|
|
595
|
-
# N = len(points)
|
|
596
|
-
# result = np.full((N, 2), np.nan, dtype=float)
|
|
597
|
-
|
|
598
|
-
# # Check for NaN input points
|
|
599
|
-
# valid = ~np.isnan(points).any(axis=1)
|
|
600
|
-
|
|
601
|
-
# if not valid.any():
|
|
602
|
-
# return result
|
|
603
|
-
|
|
604
|
-
# # Extract valid points
|
|
605
|
-
# valid_points = points[valid]
|
|
606
|
-
|
|
607
|
-
# # Vectorized expression evaluation
|
|
608
|
-
# x_gnd = valid_points[:, 0]
|
|
609
|
-
# y_gnd = valid_points[:, 1]
|
|
610
|
-
# z_gnd = valid_points[:, 2]
|
|
611
|
-
|
|
612
|
-
# # Evaluate expressions
|
|
613
|
-
# x_im = self._x_im_func(x_gnd, y_gnd, z_gnd)
|
|
614
|
-
# y_im = self._y_im_func(x_gnd, y_gnd, z_gnd)
|
|
615
|
-
|
|
616
|
-
# # Stack results and assign to valid indices
|
|
617
|
-
# result[valid] = np.column_stack([x_im, y_im])
|
|
618
|
-
|
|
619
|
-
# return result
|
|
620
|
-
|
|
621
|
-
# def gnd_to_src(self, points):
|
|
622
|
-
# """
|
|
623
|
-
# Transform from ground to source coordinates.
|
|
624
|
-
|
|
625
|
-
# Args:
|
|
626
|
-
# points: (N, 3) array of ground points
|
|
627
|
-
|
|
628
|
-
# Returns:
|
|
629
|
-
# (N, 2) array of source points
|
|
630
|
-
# """
|
|
631
|
-
# ctd_points = self.gnd_to_ctd(points)
|
|
632
|
-
# return self.ctd_to_src(ctd_points)
|
|
633
|
-
|
|
634
|
-
# def ctd_to_ray(self, points):
|
|
635
|
-
# """
|
|
636
|
-
# Transform from corrected image coordinates to 3D rays in camera space.
|
|
637
|
-
|
|
638
|
-
# Args:
|
|
639
|
-
# points: (N, 2) array of corrected image points [[x1, y1], [x2, y2], ...]
|
|
640
|
-
|
|
641
|
-
# Returns two argiments:
|
|
642
|
-
# - (3,) 3D coordinates of the key-point of the camera in world space
|
|
643
|
-
# - (N, 3) array of ray directions in camera space, with [nan, nan, nan] for invalid
|
|
644
|
-
# Ray directions are normalized to unit length.
|
|
645
|
-
# The ray for a point is defined as the vector from key-point of the camera.
|
|
646
|
-
# """
|
|
647
|
-
# points = np.asarray(points, dtype=float)
|
|
648
|
-
# if points.ndim != 2 or points.shape[1] != 2:
|
|
649
|
-
# raise ValueError(f"Expected (N, 2) array, got shape {points.shape}")
|
|
650
|
-
|
|
651
|
-
# N = len(points)
|
|
652
|
-
# ray_directions = np.full((N, 3), np.nan, dtype=float)
|
|
653
|
-
|
|
654
|
-
# # Calculate keypoint (camera position in world space)
|
|
655
|
-
# # These are typically constants, so we call with no arguments
|
|
656
|
-
# keypoint = np.array(
|
|
657
|
-
# [self._x_key_func(), self._y_key_func(), self._z_key_func()], dtype=float
|
|
658
|
-
# )
|
|
659
|
-
|
|
660
|
-
# # Check for valid points (not NaN)
|
|
661
|
-
# valid = ~np.isnan(points).any(axis=1)
|
|
662
|
-
|
|
663
|
-
# if not valid.any():
|
|
664
|
-
# return keypoint, ray_directions
|
|
665
|
-
|
|
666
|
-
# # Extract valid corrected points
|
|
667
|
-
# valid_points = points[valid]
|
|
668
|
-
|
|
669
|
-
# # Vectorized ray direction evaluation
|
|
670
|
-
# x_im = valid_points[:, 0]
|
|
671
|
-
# y_im = valid_points[:, 1]
|
|
672
|
-
|
|
673
|
-
# # Evaluate ray direction expressions
|
|
674
|
-
# ray_x = self._x_ray_func(x_im, y_im)
|
|
675
|
-
# ray_y = self._y_ray_func(x_im, y_im)
|
|
676
|
-
# ray_z = self._z_ray_func(x_im, y_im)
|
|
677
|
-
|
|
678
|
-
# # Stack ray components and ensure float64 dtype
|
|
679
|
-
# # (expressions with large integers can create object dtype arrays)
|
|
680
|
-
# rays = np.column_stack([ray_x, ray_y, ray_z]).astype(np.float64)
|
|
681
|
-
|
|
682
|
-
# # Normalize ray directions to unit length
|
|
683
|
-
# ray_lengths = np.linalg.norm(rays, axis=1, keepdims=True)
|
|
684
|
-
# # Avoid division by zero
|
|
685
|
-
# ray_lengths = np.where(ray_lengths > 0, ray_lengths, 1.0)
|
|
686
|
-
# rays_normalized = rays / ray_lengths
|
|
687
|
-
|
|
688
|
-
# # Assign normalized rays to valid indices
|
|
689
|
-
# ray_directions[valid] = rays_normalized
|
|
690
|
-
|
|
691
|
-
# return keypoint, ray_directions
|
|
692
|
-
|
|
693
|
-
# def src_to_ray(self, points):
|
|
694
|
-
# """
|
|
695
|
-
# Transform from source image coordinates to 3D rays in camera space.
|
|
696
|
-
|
|
697
|
-
# Args:
|
|
698
|
-
# points: (N, 2) array of source points [[x1, y1], [x2, y2], ...]
|
|
699
|
-
|
|
700
|
-
# Returns two argiments:
|
|
701
|
-
# - (3,) 3D coordinates of the key-point of the camera in world space
|
|
702
|
-
# - (N, 3) array of ray directions in camera space, with [nan, nan, nan] for invalid
|
|
703
|
-
# Ray directions are normalized to unit length.
|
|
704
|
-
# The ray for a point is defined as the vector from key-point of the camera.
|
|
705
|
-
# """
|
|
706
|
-
# ctd_points = self.src_to_ctd(points)
|
|
707
|
-
# return self.ctd_to_ray(ctd_points)
|
|
708
|
-
|
|
709
|
-
# @classmethod
|
|
710
|
-
# def load(cls, archive_path):
|
|
711
|
-
# """
|
|
712
|
-
# Load camera transformer from NPZ archive file.
|
|
713
|
-
|
|
714
|
-
# Args:
|
|
715
|
-
# archive_path: Path to .npz calibration file
|
|
716
|
-
|
|
717
|
-
# Returns:
|
|
718
|
-
# CameraProjectionVectorized instance
|
|
719
|
-
# """
|
|
720
|
-
|
|
721
|
-
# # Convert to regular dict for easier access
|
|
722
|
-
# cam_data = read_npz_file(archive_path)
|
|
723
|
-
|
|
724
|
-
# return cls(cam_data)
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|