Coverage for src / sdynpy / signal_processing / sdynpy_camera.py: 7%
242 statements
« prev ^ index » next coverage.py v7.13.4, created at 2026-03-11 16:22 +0000
1# -*- coding: utf-8 -*-
2"""
3Functions for dealing with camera data
4"""
5"""
6Copyright 2022 National Technology & Engineering Solutions of Sandia,
7LLC (NTESS). Under the terms of Contract DE-NA0003525 with NTESS, the U.S.
8Government retains certain rights in this software.
10This program is free software: you can redistribute it and/or modify
11it under the terms of the GNU General Public License as published by
12the Free Software Foundation, either version 3 of the License, or
13(at your option) any later version.
15This program is distributed in the hope that it will be useful,
16but WITHOUT ANY WARRANTY; without even the implied warranty of
17MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18GNU General Public License for more details.
20You should have received a copy of the GNU General Public License
21along with this program. If not, see <https://www.gnu.org/licenses/>.
22"""
24import numpy as np
25import scipy.linalg as la
26from scipy.optimize import least_squares
27from .sdynpy_rotation import matrix_to_rodrigues, rodrigues_to_matrix
30def camera_matrix_from_image(image_points, spatial_points):
31 '''Computes camera matrices from at least six points
33 This function computes camera matrices K (intrinic parameters) and R|T
34 (extrinsic parameters) from a set of points defined on the image (u,v) and
35 in 3D space (x,y,z). It sets up a system of homogeneous linear equations
36 for the camera parameters and solves for them using the singular value
37 decomposition to extract the left singular vector corresponding to the
38 smallest singular value. This vector is the closest vector to the null
39 space of the system of equations.
41 Parameters
42 ----------
43 image_points : ndarray
44 A n x 2 array where n is the number of points being correlated. The
45 first column is the horizontal pixel index from the left side of the
46 image, and the second column is the vertical pixel index from the top
47 of the image. Pixel locations can be extracted from software such as
48 Paint or GIMP, or if the image is plotted using
49 matplotlib.pyplot.imshow.
50 spatial_points : ndarray
51 A n x 3 array where n is the number of points being correlated. The
52 rows of spatial_points correpond to the rows of image_points: for a
53 point spatial_points[i,:] in 3D space, its position in the image in
54 pixels is image_points[i,:].
56 Returns
57 -------
58 K : ndarray
59 A 3 x 3 upper triangular array consisting of the camera intrinisc
60 parameters.
61 RT : ndarray
62 A 3 x 4 array where the first three columns are an orthogonal matrix
63 denoting a rotation matrix, and the last column is a translation.
65 Notes
66 -----
67 The return matrix K is of the form::
69 [f_u s u_0]
70 K = [ 0 f_v v_0]
71 [ 0 0 1 ]
73 And the matrix RT is of the form::
75 [r_xx r_xy r_xz | t_x]
76 RT = [R|T] = [r_yx r_yy r_yz | t_y]
77 [r_zx r_zy r_zz | t_z]
79 And satisfy the equation::
81 c*[u v 1].T = K@RT@[x y z 1].T
83 '''
84 # Construct the coefficient matrix
85 n_points = len(image_points)
86 coefficient_matrix = np.zeros((2 * n_points, 12))
87 for i, ([u, v], [x, y, z]) in enumerate(zip(image_points, spatial_points)):
88 coefficient_matrix[2 * i:2 * i + 2, :] = np.array([[x, y, z, 1, 0, 0, 0, 0, -u * x, -u * y, -u * z, -u],
89 [0, 0, 0, 0, x, y, z, 1, -v * x, -v * y, -v * z, -v]])
90 # Solve the homogeneous linear equations
91 [U, S, Vh] = np.linalg.svd(coefficient_matrix)
92 V = np.conj(Vh.T)
93 m = V[:, -1]
95 # Reshape back from vectorized form
96 M = m.reshape(3, 4)
98 # Decompose the matrix into an orthonormal and upper triangular matrix
99 A = M[:3, :3]
101 [K, R] = la.rq(A)
103 # Make sure that the intrinsic matrix is physical (positive focal lengths)
104 correction = np.diag([1 if K[i, i] > 0 else -1 for i in range(K.shape[0])])
105 if np.linalg.det(correction) < 0:
106 correction *= -1
108 K = K @ correction
109 R = correction @ R
111 # Assemble the final matrices
112 t = np.linalg.solve(K, M[:, -1, np.newaxis])
113 RT = np.concatenate((R, t), axis=-1)
115 # Normalize K
116 K = K / K[2, 2]
118 # Return values
119 return K, RT
122def compute_pixel_position(K, RT, coords):
123 '''Compute pixel coordinates from world coordinates
125 This function computes pixel coordinates from world coordinates and camera
126 matrices.
128 Parameters
129 ----------
130 K : np.ndarray
131 A 3x3 array of camera intrinsic parameters
132 RT : np.ndarray
133 A 3x4 array of camera extrinsic parameters
134 coords : np.ndarray
135 A 3xn array of coordinates
137 Returns
138 -------
139 pixel_position : np.ndarray
140 A (mx)2xn array of pixel positions
142 '''
143 coords_shape = np.array(coords.shape).copy()
144 coords_shape[-2] = 1
145 coords_homo = np.concatenate((coords, np.ones(coords_shape)), axis=-2)
146 pixels = K @ RT @ coords_homo
147 pixels = pixels[..., :2, :] / pixels[..., 2, np.newaxis, :]
148 return pixels
151def compute_pixel_displacement(K, RT, coords, disp):
152 '''Compute pixel displacements from coordinate displacements
154 This function computes pixel displacements from coordinate displacements
156 Parameters
157 ----------
158 K : np.ndarray
159 A 3x3 array of camera intrinsic parameters
160 RT : np.ndarray
161 A 3x4 array of camera extrinsic parameters
162 coords : np.ndarray
163 A 3xn array of coordinates
164 disp : np.ndarray
165 A (mx)3xn array of displacements
167 Returns
168 -------
169 pixel_disp : np.ndarray
170 A (mx)2xn array of pixel displacements
172 '''
173 initial_pixels = compute_pixel_position(K, RT, coords)
174 displaced_pixels = compute_pixel_position(K, RT, disp + coords)
175 return displaced_pixels - initial_pixels
178def camera_derivative_matrix(K, RT, x, y, z):
179 '''Returns a matrix of derivatives du/dx, du/dy, du/dz, dv/dx, dv/dy, dv/dz
181 Derived from derivatives of camera equation matrices. See
182 displacement_derivatives.py.
183 '''
184 return np.array(
185 [[(K[0, 0] * RT[0, 0] + K[0, 1] * RT[1, 0] + K[0, 2] * RT[2, 0])
186 / (x * K[2, 2] * RT[2, 0] + y * K[2, 2] * RT[2, 1] + z * K[2, 2] * RT[2, 2] + K[2, 2] * RT[2, 3])
187 - (x * (K[0, 0] * RT[0, 0] + K[0, 1] * RT[1, 0] + K[0, 2] * RT[2, 0])
188 + y * (K[0, 0] * RT[0, 1] + K[0, 1] * RT[1, 1] + K[0, 2] * RT[2, 1])
189 + z * (K[0, 0] * RT[0, 2] + K[0, 1] * RT[1, 2] + K[0, 2] * RT[2, 2])
190 + K[0, 0] * RT[0, 3] + K[0, 1] * RT[1, 3] + K[0, 2] * RT[2, 3]) * K[2, 2] * RT[2, 0]
191 / (x * K[2, 2] * RT[2, 0] + y * K[2, 2] * RT[2, 1] + z * K[2, 2] * RT[2, 2] + K[2, 2] * RT[2, 3])**2,
192 (K[0, 0] * RT[0, 1] + K[0, 1] * RT[1, 1] + K[0, 2] * RT[2, 1])
193 / (x * K[2, 2] * RT[2, 0] + y * K[2, 2] * RT[2, 1] + z * K[2, 2] * RT[2, 2] + K[2, 2] * RT[2, 3])
194 - (x * (K[0, 0] * RT[0, 0] + K[0, 1] * RT[1, 0] + K[0, 2] * RT[2, 0])
195 + y * (K[0, 0] * RT[0, 1] + K[0, 1] * RT[1, 1] + K[0, 2] * RT[2, 1])
196 + z * (K[0, 0] * RT[0, 2] + K[0, 1] * RT[1, 2] + K[0, 2] * RT[2, 2])
197 + K[0, 0] * RT[0, 3] + K[0, 1] * RT[1, 3] + K[0, 2] * RT[2, 3]) * K[2, 2] * RT[2, 1]
198 / (x * K[2, 2] * RT[2, 0] + y * K[2, 2] * RT[2, 1] + z * K[2, 2] * RT[2, 2] + K[2, 2] * RT[2, 3])**2,
199 (K[0, 0] * RT[0, 2] + K[0, 1] * RT[1, 2] + K[0, 2] * RT[2, 2])
200 / (x * K[2, 2] * RT[2, 0] + y * K[2, 2] * RT[2, 1] + z * K[2, 2] * RT[2, 2] + K[2, 2] * RT[2, 3])
201 - (x * (K[0, 0] * RT[0, 0] + K[0, 1] * RT[1, 0] + K[0, 2] * RT[2, 0])
202 + y * (K[0, 0] * RT[0, 1] + K[0, 1] * RT[1, 1] + K[0, 2] * RT[2, 1])
203 + z * (K[0, 0] * RT[0, 2] + K[0, 1] * RT[1, 2] + K[0, 2] * RT[2, 2])
204 + K[0, 0] * RT[0, 3] + K[0, 1] * RT[1, 3] + K[0, 2] * RT[2, 3]) * K[2, 2] * RT[2, 2]
205 / (x * K[2, 2] * RT[2, 0] + y * K[2, 2] * RT[2, 1] + z * K[2, 2] * RT[2, 2] + K[2, 2] * RT[2, 3])**2
206 ],
207 [(K[1, 1] * RT[1, 0] + K[1, 2] * RT[2, 0])
208 / (x * K[2, 2] * RT[2, 0] + y * K[2, 2] * RT[2, 1] + z * K[2, 2] * RT[2, 2] + K[2, 2] * RT[2, 3])
209 - (x * (K[1, 1] * RT[1, 0] + K[1, 2] * RT[2, 0])
210 + y * (K[1, 1] * RT[1, 1] + K[1, 2] * RT[2, 1])
211 + z * (K[1, 1] * RT[1, 2] + K[1, 2] * RT[2, 2])
212 + K[1, 1] * RT[1, 3] + K[1, 2] * RT[2, 3]) * K[2, 2] * RT[2, 0]
213 / (x * K[2, 2] * RT[2, 0] + y * K[2, 2] * RT[2, 1] + z * K[2, 2] * RT[2, 2] + K[2, 2] * RT[2, 3])**2,
214 (K[1, 1] * RT[1, 1] + K[1, 2] * RT[2, 1])
215 / (x * K[2, 2] * RT[2, 0] + y * K[2, 2] * RT[2, 1] + z * K[2, 2] * RT[2, 2] + K[2, 2] * RT[2, 3])
216 - (x * (K[1, 1] * RT[1, 0] + K[1, 2] * RT[2, 0])
217 + y * (K[1, 1] * RT[1, 1] + K[1, 2] * RT[2, 1])
218 + z * (K[1, 1] * RT[1, 2] + K[1, 2] * RT[2, 2])
219 + K[1, 1] * RT[1, 3] + K[1, 2] * RT[2, 3]) * K[2, 2] * RT[2, 1]
220 / (x * K[2, 2] * RT[2, 0] + y * K[2, 2] * RT[2, 1] + z * K[2, 2] * RT[2, 2] + K[2, 2] * RT[2, 3])**2,
221 (K[1, 1] * RT[1, 2] + K[1, 2] * RT[2, 2])
222 / (x * K[2, 2] * RT[2, 0] + y * K[2, 2] * RT[2, 1] + z * K[2, 2] * RT[2, 2] + K[2, 2] * RT[2, 3])
223 - (x * (K[1, 1] * RT[1, 0] + K[1, 2] * RT[2, 0]) + y * (K[1, 1] * RT[1, 1] + K[1, 2] * RT[2, 1])
224 + z * (K[1, 1] * RT[1, 2] + K[1, 2] * RT[2, 2]) + K[1, 1] * RT[1, 3] + K[1, 2] * RT[2, 3]) * K[2, 2] * RT[2, 2]
225 / (x * K[2, 2] * RT[2, 0] + y * K[2, 2] * RT[2, 1] + z * K[2, 2] * RT[2, 2] + K[2, 2] * RT[2, 3])**2
226 ]]
227 )
230def homogeneous_coordinates(array):
231 shape = np.array(array.shape).copy()
232 shape[-2] = 1
233 return np.concatenate((array, np.ones(shape)), axis=-2)
236def unhomogeneous_coordinates(array):
237 return array[..., :-1, :] / array[..., -1:, :]
240def point_on_pixel(K, RT, pixel, distance=1):
241 """
242 Compute the 3D position of a point on a pixel of a given camera
244 Parameters
245 ----------
246 K : np.ndarray
247 A 3x3 array of camera intrinsic parameters
248 RT : np.ndarray
249 A 3x4 array of camera extrinsic parameters
250 pixel : ndarray
251 A (...,2) shape ndarray where the last dimension contains pixel positions
252 distance : float, optional
253 The distance that the point will be positioned from the camera pinhole.
254 The default is 1.
256 Returns
257 -------
258 pixel_coordinates : ndarray
259 An array of 3D coordinates with the same shape as the original pixel
260 array, where the 2D pixel position has been replaced with a 3D X,Y,Z
261 position of the point
263 """
264 pixel = np.array(pixel)
265 pixel_center_positions = pixel - K[:2, 2]
267 ray_positions = np.concatenate((
268 np.linalg.solve(K[:2, :2], pixel_center_positions[..., np.newaxis]),
269 np.ones(pixel.shape[:-1] + (1, 1))
270 ), axis=-2)
272 ray_positions_homogeneous = np.concatenate((
273 ray_positions,
274 np.ones(pixel.shape[:-1] + (1, 1))
275 ), axis=-2)
277 RT_homogeneous = np.concatenate((RT, np.array([[0, 0, 0, 1]])), axis=-2)
279 ray_positions_3D_homogeneous = np.linalg.solve(RT_homogeneous, ray_positions_homogeneous)
280 # Convert back to normal coordinates
281 ray_positions_3D = ray_positions_3D_homogeneous[...,
282 :3, :] / ray_positions_3D_homogeneous[..., 3:, :]
284 pinhole = -RT[:3, :3].T @ RT[:3, 3:]
286 ray_unit_vectors = ray_positions_3D - pinhole
287 ray_unit_vectors = ray_unit_vectors / np.linalg.norm(ray_unit_vectors, axis=-2, keepdims=True)
289 pixel_coordinates = distance * ray_unit_vectors + pinhole
290 return pixel_coordinates[..., 0]
293def distort_points(image_points, K=None, k=np.zeros(6), p=np.zeros(2), s=np.zeros(4)):
294 if K is None:
295 normalized_image_points = image_points
296 else:
297 normalized_image_points = image_points - K[..., :2, 2]
298 normalized_image_points = np.einsum(
299 '...ij,...j->...i', np.linalg.inv(K[..., :2, :2]), normalized_image_points)
300 r = np.linalg.norm(normalized_image_points, axis=-1)
301 r2 = r**2
302 r4 = r**4
303 r6 = r**6
304 xp = normalized_image_points[..., 0]
305 yp = normalized_image_points[..., 1]
306 xp2 = xp**2
307 yp2 = yp**2
308 xpp = (xp * (1 + k[..., 0] * r2 + k[..., 1] * r4 + k[..., 2] * r6) / (1 + k[..., 3] * r2 + k[..., 4] * r4 + k[..., 5] * r6)
309 + 2 * p[..., 0] * xp * yp + p[..., 1] * (r2 + 2 * xp2)
310 + s[..., 0] * r2 + s[..., 1] * r4)
311 ypp = (yp * (1 + k[..., 0] * r2 + k[..., 1] * r4 + k[..., 2] * r6) / (1 + k[..., 3] * r2 + k[..., 4] * r4 + k[..., 5] * r6)
312 + 2 * p[..., 1] * xp * yp + p[..., 0] * (r2 + 2 * yp2)
313 + s[..., 2] * r2 + s[..., 3] * r4)
314 distorted_normalized_points = np.concatenate(
315 (xpp[..., np.newaxis], ypp[..., np.newaxis]), axis=-1)
316 if K is None:
317 return distorted_normalized_points
318 else:
319 return np.einsum('...ij,...j->...i', K[..., :2, :2], distorted_normalized_points) + K[..., :2, 2]
322def calibration_linear_estimate(image_points, plane_points):
323 """
324 Computes a linear estimate of the camera parameters from point correspondences
326 Uses pass in a set of points on images for multiple views of the same
327 calibration object
329 Parameters
330 ----------
331 image_points : ndarray
332 A (n_camera x) n_image x n_point x 2 array of image points
333 plane_points : ndarray
334 A n_point x 2 array of 2D positions of the points on the calibration
335 object
337 Raises
338 ------
339 ValueError
340 If the point correspondence matrices are not the correct sizes.
342 Returns
343 -------
344 K_est
345 A (n_camera x) 3 x 3 estimate of the camera intrinsic matrix
346 RT_est
347 A (n_camera x) n_image x 3 x 4 estimate of the camera extrinsic matrix
348 """
349 original_shape = image_points.shape[:-2]
351 if image_points.ndim < 3:
352 raise ValueError(
353 'image_points must have shape (n_images x n_points x 2) or (n_cameras x n_images x n_points x 2)')
354 if image_points.ndim == 3:
355 image_points = image_points[np.newaxis, ...]
357 n_cameras, n_images, n_points, _ = image_points.shape
359 # First let's go through and find the mask where the points are not defined
360 # or are off the image
361 valid_points = ~np.any(np.isnan(image_points), axis=-1)
363 # Now let's compute the homography
364 plane_points_h = np.concatenate(
365 (plane_points, np.ones(plane_points.shape[:-1] + (1,))), axis=-1).T
366 zero = np.zeros(plane_points_h.shape)
368 homography_guess_matrix = np.array([np.block([
369 [plane_points_h.T, zero.T, -points[:, 0, np.newaxis] * plane_points_h.T],
370 [zero.T, plane_points_h.T, -points[:, 1, np.newaxis] * plane_points_h.T]
371 ]) for points in image_points.reshape(-1, n_points, 2)])
373 # Go through and compute the homography transformation for each camera and each
374 # image
375 homography = []
376 for index, (guess_matrix, mask) in enumerate(zip(homography_guess_matrix, valid_points.reshape(n_cameras * n_images, n_points))):
377 guess_matrix = guess_matrix[np.concatenate((mask, mask))]
378 if guess_matrix.shape[0] < 9:
379 homography.append(np.nan * np.ones(9))
380 continue
381 column_preconditioner = np.diag(1 / np.sqrt(np.mean(guess_matrix**2, axis=-2)))
382 row_preconditioner = np.diag(1 / np.sqrt(np.mean(guess_matrix**2, axis=-1)))
383 homography_guess_matrix_normalized = row_preconditioner @ guess_matrix @ column_preconditioner
384 _, _, Vh = np.linalg.svd(homography_guess_matrix_normalized)
385 homography_guesses = column_preconditioner @ Vh[-1, :]
386 # Now do the nonlinear updating
387 valid_image_points = image_points[index // n_images, index % n_images, mask].T
388 valid_plane_points = plane_points_h[:, mask]
390 def optfunc(h_vect):
391 return np.linalg.norm(valid_image_points - (h_vect[0:6].reshape(2, 3) @ valid_plane_points)
392 / (h_vect[6:9].reshape(1, 3) @ valid_plane_points), axis=0)
393 soln = least_squares(optfunc, homography_guesses, method='lm')
394 homography.append(soln.x)
396 homography = np.array(homography).reshape(n_cameras, n_images, 3, 3)
398 # Set up constraint matrix (V in zhang)
399 def intrinsic_constraint(i, j): return np.moveaxis(np.array([homography[..., 0, i] * homography[..., 0, j],
400 homography[..., 0, i] * homography[..., 1, j] +
401 homography[..., 1, i] *
402 homography[..., 0, j],
403 homography[..., 1, i] *
404 homography[..., 1, j],
405 homography[..., 2, i] * homography[..., 0, j] +
406 homography[..., 0, i] *
407 homography[..., 2, j],
408 homography[..., 2, i] * homography[..., 1, j] +
409 homography[..., 1, i] *
410 homography[..., 2, j],
411 homography[..., 2, i] * homography[..., 2, j]]), 0, -1)
413 intrinsic_constraint_matrix = np.block([
414 [intrinsic_constraint(0, 1)],
415 [intrinsic_constraint(0, 0) - intrinsic_constraint(1, 1)]
416 ])
418 K_est = []
419 RT_est = []
421 for this_intrinsic_constraint_matrix, this_homography in zip(intrinsic_constraint_matrix, homography):
422 good_image_rows = ~np.any(np.isnan(this_intrinsic_constraint_matrix), axis=1)
423 U, S, Vh = np.linalg.svd(this_intrinsic_constraint_matrix[good_image_rows])
424 # Intrinsic parameter vector (b in zhang)
425 intrinsic_parameters = Vh[..., -1, :]
427 [B11, B12, B22, B13, B23, B33] = intrinsic_parameters.T
429 cy = (B12 * B13 - B11 * B23) / (B11 * B22 - B12**2)
430 lam = B33 - (B13**2 + cy * (B12 * B13 - B11 * B23)) / B11
431 fx = np.sqrt(lam / B11)
432 fy = np.sqrt(lam * B11 / (B11 * B22 - B12**2))
433 s = -B12 * fx**2 * fy / lam
434 cx = s * cy / fx - B13 * fx**2 / lam
436 K_est.append(np.array([
437 [fx, s, cx],
438 [0, fy, cy],
439 [0, 0, 1]]))
441 # extrinsics_estimate = np.linalg.solve(K_est[:,np.newaxis],homography)
442 extrinsics_estimate = np.linalg.solve(K_est[-1], this_homography)
443 this_RT = np.nan * np.ones((n_images, 3, 4))
444 good_image_rows = np.where(~np.any(np.isnan(extrinsics_estimate), axis=(1, 2)))[0]
445 extrinsics_estimate = extrinsics_estimate[good_image_rows]
446 r1 = extrinsics_estimate[..., 0]
447 r2 = extrinsics_estimate[..., 1]
448 scale_factor_1 = np.linalg.norm(r1, axis=-1, keepdims=True)
449 scale_factor_2 = np.linalg.norm(r2, axis=-1, keepdims=True)
450 scale_factor = (scale_factor_1 + scale_factor_2) / 2
451 r1 /= scale_factor
452 r2 /= scale_factor
453 r3 = np.cross(r1, r2)
454 t = extrinsics_estimate[..., 2]
455 t /= scale_factor
456 U, S, Vh = np.linalg.svd(np.concatenate((r1[..., np.newaxis],
457 r2[..., np.newaxis],
458 r3[..., np.newaxis]), axis=-1))
459 R_est = U @ Vh
460 RT_est_0 = np.concatenate((R_est, t[..., np.newaxis]), axis=-1)
462 r1 = -r1
463 r2 = -r2
464 t = -t
465 r3 = np.cross(r1, r2)
466 U, S, Vh = np.linalg.svd(np.concatenate((r1[..., np.newaxis],
467 r2[..., np.newaxis],
468 r3[..., np.newaxis]), axis=-1))
469 R_est = U @ Vh
470 RT_est_1 = np.concatenate((R_est, t[..., np.newaxis]), axis=-1)
472 check_points = RT_est_0 @ np.concatenate((plane_points, np.zeros(
473 (plane_points.shape[0], 1)), np.ones((plane_points.shape[0], 1))), axis=-1).T
474 use_negative = np.any(check_points[..., 2, :] < 0, axis=-1)
476 this_RT[good_image_rows[use_negative]] = RT_est_1[use_negative]
477 this_RT[good_image_rows[~use_negative]] = RT_est_0[~use_negative]
479 RT_est.append(this_RT)
481 return np.array(K_est).reshape(*original_shape[:-1], 3, 3), np.array(RT_est).reshape(*original_shape, 3, 4)
484def reconstruct_scene_from_calibration_parameters(parameter_array, n_cameras, n_images,
485 radial_distortions=0,
486 prismatic_distortions=0,
487 tangential_distortions=0,
488 radial_denominator_distortions=False,
489 use_K_for_distortions=True):
490 index = 0
491 # Intrinsic Parameters
492 n_parameters = n_cameras * 5
493 K = np.tile(np.eye(3), (n_cameras, 1, 1))
494 K[:, [0, 1, 0, 0, 1], [0, 1, 1, 2, 2]] = parameter_array[index:index +
495 n_parameters].reshape(n_cameras, 5)
496 index += n_parameters
497 # Camera extrinsics
498 RT_cameras = np.tile(np.eye(3, 4), (n_cameras, 1, 1))
499 # Camera Rotations
500 n_parameters = (n_cameras - 1) * 3
501 rvecs = parameter_array[index:index + n_parameters].reshape((n_cameras - 1), 3)
502 R = rodrigues_to_matrix(rvecs)
503 RT_cameras[1:, :3, :3] = R
504 index += n_parameters
505 # Camera translations
506 RT_cameras[1:, :, 3] = parameter_array[index:index + n_parameters].reshape(n_cameras - 1, 3)
507 index += n_parameters
508 # Image extrinsics
509 RT_images = np.tile(np.eye(4, 4), (n_images, 1, 1))
510 # Image rotations
511 n_parameters = n_images * 3
512 rvecs = parameter_array[index:index + n_parameters].reshape(n_images, 3)
513 R = rodrigues_to_matrix(rvecs)
514 RT_images[:, :3, :3] = R
515 index += n_parameters
516 # Image translations
517 RT_images[:, :3, 3] = parameter_array[index:index + n_parameters].reshape(n_images, 3)
518 index += n_parameters
519 # Radial distortion parameters
520 radial_distortion_parameters = np.zeros((n_cameras, 6))
521 if radial_denominator_distortions:
522 n_parameters = radial_distortions * 2 * n_cameras
523 params = parameter_array[index:index +
524 n_parameters].reshape(n_cameras, radial_distortions * 2)
525 radial_distortion_parameters[:, :radial_distortions] = params[:, :params.shape[-1] // 2]
526 radial_distortion_parameters[:, 3:3 +
527 radial_distortions] = params[:, params.shape[-1] // 2:]
528 else:
529 n_parameters = radial_distortions * n_cameras
530 params = parameter_array[index:index + n_parameters].reshape(n_cameras, radial_distortions)
531 radial_distortion_parameters[:, :radial_distortions] = params
532 index += n_parameters
533 # Prismatic distortions
534 prismatic_distortion_parameters = np.zeros((n_cameras, 4))
535 n_parameters = prismatic_distortions * 2 * n_cameras
536 prismatic_distortion_parameters[:, :prismatic_distortions *
537 2] = parameter_array[index:index + n_parameters].reshape(n_cameras, prismatic_distortions * 2)
538 if prismatic_distortions == 1:
539 prismatic_distortion_parameters = prismatic_distortion_parameters[:, [0, 2, 1, 3]]
540 index += n_parameters
541 # Tangential Distortions
542 tangential_distortion_parameters = np.zeros((n_cameras, 2))
543 n_parameters = tangential_distortions * n_cameras
544 tangential_distortion_parameters[:, :tangential_distortions] = parameter_array[index:index +
545 n_parameters].reshape(n_cameras, tangential_distortions)
546 index += n_parameters
547 # K matrix for distortions (if used)
548 if use_K_for_distortions:
549 K_distortion = None
550 else:
551 n_parameters = n_cameras * 5
552 K_distortion = np.tile(np.eye(3), (n_cameras, 1, 1))
553 K_distortion[:, [0, 1, 0, 0, 1], [0, 1, 1, 2, 2]
554 ] = parameter_array[index:index + n_parameters].reshape(n_cameras, 5)
555 index += n_parameters
556 return (K, RT_cameras, RT_images, radial_distortion_parameters,
557 prismatic_distortion_parameters, tangential_distortion_parameters,
558 K_distortion)
561def optimize_calibration(image_points, plane_points, K_guess, RT_guess, radial_distortions=0,
562 prismatic_distortions=0, tangential_distortions=0,
563 radial_denominator_distortions=False,
564 K_guess_distortion=None, **least_squares_kwargs):
565 n_cameras, n_images, n_points, _ = image_points.shape
566 # Set up initial rotation guesses
567 RT_guess_cameras = np.empty((n_cameras - 1, 3, 4))
568 RT_guess_images = np.empty((n_images, 3, 4))
569 valid_images = ~np.any(np.isnan(RT_guess), axis=(-1, -2))
570 # Set up the initial camera transformations
571 for i in range(n_cameras - 1):
572 for j in range(n_images):
573 if valid_images[i + 1, j] and valid_images[0, j]:
574 global_transformation = np.linalg.inv(
575 np.concatenate((RT_guess[0, j], [[0, 0, 0, 1]])))
576 RT_guess_cameras[i] = RT_guess[i + 1, j] @ global_transformation
577 # Set up the initial image transformations
578 for j in range(n_images):
579 for i in range(n_cameras):
580 if valid_images[i, j]:
581 if i == 0:
582 RT_guess_images[j] = RT_guess[i, j]
583 else:
584 RT_guess_images[j] = (np.linalg.inv(np.concatenate(
585 (RT_guess_cameras[i - 1], [[0, 0, 0, 1]]))) @ np.concatenate((RT_guess[i, j], [[0, 0, 0, 1]])))[:3]
586 # # Check that this is right
587 # RT_1 = np.concatenate((np.eye(3,4)[np.newaxis],RT_guess_cameras),axis=0)
588 # RT_2 = np.concatenate((RT_guess_images,np.tile(np.array((0,0,0,1)),(15,1,1))),axis=-2)
589 # RT_guess_check = RT_1[:,np.newaxis]@RT_2
590 initial_guesses = np.concatenate([
591 # Intrinsic parameters fx,fy,s,cx,cy
592 K_guess[:, [0, 1, 0, 0, 1], [0, 1, 1, 2, 2]].flatten(),
593 # Rotations between cameras coordinate systems
594 matrix_to_rodrigues(RT_guess_cameras[..., :3, :3]).flatten(),
595 RT_guess_cameras[..., :, 3].flatten(), # Translations between camera coordinate systems
596 # Rotations of plane at each image
597 matrix_to_rodrigues(RT_guess_images[:, :3, :3]).flatten(),
598 RT_guess_images[:, :3, 3].flatten(), # Translations of plane at each image
599 np.zeros(radial_distortions * (2 if radial_denominator_distortions else 1)
600 * n_cameras), # Radial distortions
601 np.zeros(prismatic_distortions * 2 * n_cameras), # Prismatic distortions
602 np.zeros(tangential_distortions * n_cameras), # tangential distortions
603 [] if K_guess_distortion is None else K_guess_distortion[:, [0, 1, 0, 0, 1], [0, 1, 1, 2, 2]].flatten()
604 ])
605 variable_scale = np.concatenate([
606 1000 * np.ones(n_cameras * 5), # Intrinsic parameters fx,fy,s,cx,cy, in the 1000s of pixels
607 # Rotatons between cameras coordinate systems, generally between -pi and pi
608 np.ones((n_cameras - 1) * 3),
609 # Translations bewteen cameras, assume approximately 1 meter distances
610 np.ones((n_cameras - 1) * 3),
611 np.ones(n_images * 3), # Rotations of the plane at each image, generally between -pi and pi
612 # Translations of the plane at each image, assume 10 centimeters
613 0.1 * np.ones(n_images * 3),
614 0.1 * np.ones(radial_distortions * (2 if radial_denominator_distortions else 1)
615 * n_cameras), # Radial distortions
616 0.1 * np.ones(prismatic_distortions * 2 * n_cameras), # Prismatic distortions
617 0.1 * np.ones(tangential_distortions * n_cameras), # tangential distortions
618 # Intrinsic parameters used in distortion calculations
619 [] if K_guess_distortion is None else 1000 * np.ones(n_cameras * 5)
620 ])
621 plane_points_h = np.concatenate((plane_points.T,
622 np.zeros((1, plane_points.shape[0])),
623 np.ones((1, plane_points.shape[0]))))
624 print('Optimizing {:} degrees of freedom'.format(initial_guesses.size))
626 def error_func(x):
627 # Get scene parameters
628 (K, RT_cameras, RT_images, radial_distortion_parameters,
629 prismatic_distortion_parameters, tangential_distortion_parameters,
630 K_distortion) = reconstruct_scene_from_calibration_parameters(
631 x, n_cameras, n_images,
632 radial_distortions,
633 prismatic_distortions,
634 tangential_distortions,
635 radial_denominator_distortions,
636 K_guess_distortion is None)
637 # Project points through camera equations
638 image_points_reproj_h = K[:, np.newaxis] @ RT_cameras[:,
639 np.newaxis] @ RT_images @ plane_points_h
640 image_points_reproj = np.moveaxis(
641 image_points_reproj_h[..., :2, :] / image_points_reproj_h[..., 2:, :], -2, -1)
642 # Deform the points
643 if np.any(radial_distortion_parameters != 0.0) or np.any(prismatic_distortion_parameters != 0.0) or np.any(prismatic_distortion_parameters != 0.0):
644 image_points_reproj = distort_points(
645 image_points_reproj,
646 K[:, np.newaxis, np.newaxis] if K_distortion is None else K_distortion[:, np.newaxis, np.newaxis],
647 radial_distortion_parameters[:, np.newaxis, np.newaxis],
648 tangential_distortion_parameters[:, np.newaxis, np.newaxis],
649 prismatic_distortion_parameters[:, np.newaxis, np.newaxis])
650 # Compute the residual
651 residual = image_points - image_points_reproj
652 return residual[~np.isnan(residual)]
654 # Now optimize
655 soln = least_squares(error_func, initial_guesses, method='lm',
656 x_scale=variable_scale, **least_squares_kwargs)
657 (K, RT_cameras, RT_images, radial_distortion_parameters,
658 prismatic_distortion_parameters, tangential_distortion_parameters,
659 K_distortion) = reconstruct_scene_from_calibration_parameters(
660 soln.x, n_cameras, n_images,
661 radial_distortions,
662 prismatic_distortions,
663 tangential_distortions,
664 radial_denominator_distortions,
665 K_guess_distortion is None)
666 return (K, RT_cameras, RT_images, radial_distortion_parameters,
667 prismatic_distortion_parameters, tangential_distortion_parameters,
668 K_distortion)
671def decomposeP(P):
672 """
673 Decomposes a projection matrix P into intrinsic and extrinsic matrices K, R, and t
675 Parameters
676 ----------
677 P : np.ndarray
678 3x4 camera projection matrix P = K@np.concatenate((R,t),axis=-1)
680 Returns
681 -------
682 K : ndarray
683 3x3 upper triangular intrinsic parameter matrix
684 R : ndarray
685 3x3 rotation matrix
686 t : ndarray
687 3x1 translation vector
689 """
690 M = P[0:3, 0:3]
691 Q = np.eye(3)[:: -1]
692 P_b = Q @ M @ M.T @ Q
693 K_h = Q @ np.linalg.cholesky(P_b) @ Q
694 K = K_h / K_h[2, 2]
695 A = np.linalg .inv(K) @ M
696 L = (1 / np.linalg.det(A))**(1 / 3)
697 R = L * A
698 t = L * np.linalg.inv(K) @ P[0:3, 3]
699 return K, R, t