Skip to content

Free Motion Capture for Everyone πŸ’€βœ¨

TypeScript

lets call it here and try shoving data through it tomorrow

jonmatthis82a7ae18fc17
diff --git a/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/anipose_camera_group.py b/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/anipose_camera_group.py
index c0086165..25c687a4 100644
--- a/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/anipose_camera_group.py
+++ b/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/anipose_camera_group.py
@@ -2,6 +2,7 @@
# ore info on Anipoise: https://anipose.readthedocs.io/en/latest/
import logging
+from copy import deepcopy
from dataclasses import dataclass
from typing import Any
@@ -37,13 +38,61 @@ def anipose_triangulate_simple(points2d: np.ndarray, camera_matrices: np.ndarray
A[i * 2] = x * mat[2] - mat[0]
A[i * 2 + 1] = y * mat[2] - mat[1]
- _, _, vh = np.linalg.svd(A, full_matrices=False) #CHECK - Ai said this could be set to False since " we only need the right singular vectors, which is more efficient." but we should check on this bc I don't know what that means lol
+ _, _, vh = np.linalg.svd(A,
+ full_matrices=False) # CHECK - Ai said this could be set to False since " we only need the right singular vectors, which is more efficient." but we should check on this bc I don't know what that means lol
points3d_xyzh = vh[-1]
points3d_xyzh /= points3d_xyzh[3] # Normalize the last element to 1
return points3d_xyzh[:3]
+
+@dataclass
+class CalibrationInputData:
+ pixel_points2d: np.ndarray # Shape: (N_cams, N_points, 2), px, py pixel coordinates of object points
+ object_points3d: np.ndarray # Shape: (N_points, 3), x, y, z object points in 3D space (corresponding to pixel_points2d)
+ ids: np.ndarray # IDs of points (2d and 3d) for each camera
+ rotation_vectors: np.ndarray # Rotation vectors
+ translation_vectors: np.ndarray # Translation vectors
+
+ def __post_init__(self):
+ number_of_points = self.pixel_points2d.shape[1]
+ if not self.pixel_points2d.ndim == 3 and self.pixel_points2d.shape[2] == 2:
+ raise ValueError(
+ f"Invalid pixel_points2d shape, expected shape to be (N_cams, N_points, 2), but got shape {self.pixel_points2d.shape}"
+ )
+
+ if not self.object_points3d.shape[0] == number_of_points:
+ raise ValueError(
+ f"Invalid object_points3d shape, expected first dimension to be equal to "
+ f"number of points ({number_of_points}), but got shape {self.object_points3d.shape}"
+ )
+ if not self.ids.shape[0] == number_of_points:
+ raise ValueError(
+ f"Invalid ids shape, expected first dimension to be equal to "
+ f"number of points ({number_of_points}), but got shape {self.ids.shape}"
+ )
+ if not self.rotation_vectors.shape[0] == len(self.pixel_points2d):
+ raise ValueError(
+ f"Invalid rotation_vectors shape, expected first dimension to be equal to "
+ f"number of cameras ({len(self.pixel_points2d)}), but got shape {self.rotation_vectors.shape}"
+ )
+ if not self.translation_vectors.shape[0] == len(self.pixel_points2d):
+ raise ValueError(
+ f"Invalid translation_vectors shape, expected first dimension to be equal to "
+ f"number of cameras ({len(self.pixel_points2d)}), but got shape {self.translation_vectors.shape}"
+ )
+
+ def resample_indicies(self, indicies: list[int]):
+ return CalibrationInputData(
+ pixel_points2d=self.pixel_points2d[:, indicies],
+ object_points3d=self.object_points3d[indicies],
+ ids=self.ids[indicies],
+ rotation_vectors=self.rotation_vectors[:, indicies],
+ translation_vectors=self.translation_vectors[:, indicies],
+ )
+
+
def calculate_error_bounds(error_dict: dict[tuple[int, int], tuple[int, np.ndarray]]) -> tuple[float, float]:
"""Calculate the maximum and minimum error bounds from an error dictionary.
@@ -79,7 +128,8 @@ def transform_points(points, rotation_vectors, translation_vectors):
return rotated + translation_vectors
-def remap_ids(ids: np.ndarray|list[int]) -> np.ndarray:
+
+def remap_ids(ids: np.ndarray | list[int]) -> np.ndarray:
"""
Remap the unique identifiers in the input array to a consecutive range starting from zero.
@@ -103,20 +153,13 @@ def subset_extra_data(extra_data: dict[str, Any], indices: np.ndarray) -> dict[s
return extra_data
return {key: value[indices] for key, value in extra_data.items() if key in extra_data}
-def resample_points_based_on_shared_views(points2d: np.ndarray,
- extra_data: dict[str, Any],
- number_of_samples_to_return: int = 25) -> tuple[np.ndarray, dict[str, Any] | None]:
+
+def resample_points_based_on_shared_views(calibration_input_data: CalibrationInputData,
+ number_of_samples_to_return: int = 25) -> CalibrationInputData:
"""
- Resample 2D points to prioritize those seen by multiple cameras.
-
- :param points2d: A 3D array of shape (N_cams, N_points, 2) containing 2D points for each camera.
- NaN values represent missing data for a camera.
- :param extra_data: Optional dictionary containing additional data (like {'objp', 'ids', 'rotation_vectors', 'translation_vectors'})
- that should be resampled in the same way as points2d.
- :param number_of_samples_to_return: The maximum number of points to sample, prioritizing those visible in more cameras.
- :return: A tuple containing the resampled points2d and the resampled extra dictionary.
+ Resample calibration input data prioritize 2d points seen by multiple cameras.
"""
-
+ points2d = calibration_input_data.pixel_points2d
n_cams = points2d.shape[0]
points2d_valid = ~np.isnan(points2d[:, :, 0]) # Boolean array indicating valid (non-NaN) points
point_indices = np.arange(points2d.shape[1])
@@ -145,37 +188,30 @@ def resample_points_based_on_shared_views(points2d: np.ndarray,
# Sort the final indices of included points for consistent ordering
final_indices = sorted(included_points)
- resampled_ponts = points2d[:, final_indices]
- extra_data = subset_extra_data(extra_data, final_indices)
+ return calibration_input_data.resample_indicies(final_indices)
- return resampled_ponts, extra_data
-def subset_extra(extra, ixs):
- if extra is None:
- return None
+def construct_camera_extrinsics_matrix(rotation_vector: np.ndarray,
+ translation_vector: np.ndarray) -> np.ndarray:
+ if rotation_vector.shape != (3,) or translation_vector.shape != (3,):
+ raise ValueError("Rotation vector and translation vector must have shape (3,)")
+ extrinsics_matrix = np.zeros((4, 4))
+ rotmat, _ = cv2.Rodrigues(rotation_vector)
+ extrinsics_matrix[:3, :3] = rotmat
+ extrinsics_matrix[:3, 3] = translation_vector.flatten()
+ extrinsics_matrix[3, 3] = 1
+ return extrinsics_matrix
- new_extra = {
- "objp": extra["objp"][ixs],
- "ids": extra["ids"][ixs],
- "rotation_vectors": extra["rotation_vectors"][:, ixs],
- "translation_vectors": extra["translation_vectors"][:, ixs],
- }
- return new_extra
+def get_rotation_and_translation_vector_from_extrinsics_matrix(extrinsics_matrix: np.ndarray) -> tuple[
+ np.ndarray, np.ndarray]:
+ if extrinsics_matrix.shape != (4, 4):
+ raise ValueError("Extrinsics matrix must be a 4x4 matrix")
+ rotation_vector = cv2.Rodrigues(extrinsics_matrix[:3, :3])[0].flatten()
+ translation_vector = extrinsics_matrix[:3, 3].flatten()
+ return rotation_vector, translation_vector
-def make_M(rvec, tvec):
- out = np.zeros((4,4))
- rotmat, _ = cv2.Rodrigues(rvec)
- out[:3,:3] = rotmat
- out[:3, 3] = tvec.flatten()
- out[3, 3] = 1
- return out
-
-def get_rtvec(M):
- rvec = cv2.Rodrigues(M[:3, :3])[0].flatten()
- tvec = M[:3, 3].flatten()
- return rvec, tvec
def get_error_dict(errors_full, min_points=10):
n_cams = errors_full.shape[0]
@@ -196,13 +232,14 @@ def get_error_dict(errors_full, min_points=10):
error_dict[(i, j)] = (err_subset.shape[1], percents)
return error_dict
+
@dataclass
class AniposeCameraCalibrationEstimate:
name: str
camera_id: CameraId
size: tuple[int, int]
- rotation_vector: np.ndarray # rodriguez rotation vector relative to Camera#0
- translation_vector: np.ndarray # translation vector relative to Camera#0
+ rotation_vector: np.ndarray
+ translation_vector: np.ndarray
camera_matrix: np.ndarray = np.eye(3, dtype="float64")
distortion_coefficients: np.ndarray = np.zeros(5),
@@ -235,8 +272,6 @@ class AniposeCameraCalibrationEstimate:
extrinsics_matrix[3, 3] = 1
return extrinsics_matrix
-
-
def get_optimizable_parameters(self):
"""
Grabs the parametesr that will be optimed and organizes them in a numpy array
@@ -268,7 +303,6 @@ class AniposeCameraCalibrationEstimate:
distortion_coefficients[1] = optimized_parameters[8]
self.distortion_coefficients = distortion_coefficients
-
def undistort_points(self, points2d: np.ndarray) -> np.ndarray:
"""
Undistorts 2D points using the camera's intrinsic parameters and distortion coefficients.
@@ -305,25 +339,18 @@ class AniposeCameraCalibrationEstimate:
return projected_2d_points
- def single_camera_reprojection_error(self, points3d:np.ndarray, points2d:np.ndarray) -> np.ndarray:
+ def single_camera_reprojection_error(self, points3d: np.ndarray, points2d: np.ndarray) -> np.ndarray:
projected_points3d = self.project_3d_to_2d(points3d)
- projecting_3d_points_onto_2d_image_plane = projected_points3d.reshape(points2d.shape) #? Should this be np.squeeze(projected_points3d)?
+ projecting_3d_points_onto_2d_image_plane = projected_points3d.reshape(
+ points2d.shape) # ? Should this be np.squeeze(projected_points3d)?
return points2d - projecting_3d_points_onto_2d_image_plane
-@dataclass
-class CalibrationInputData:
- pixel_points2d: np.ndarray # Shape: (N_cams, N_points, 2), px, py pixel coordinates of object points
- object_points3d: np.ndarray # Shape: (N_points, 3), x, y, z object points in 3D space (corresponding to pixel_points2d)
- ids: np.ndarray # IDs of points (2d and 3d) for each camera
- rotation_vectors: np.ndarray # Rotation vectors
- translation_vectors: np.ndarray # Translation vectors
-
@dataclass
class AniposeCameraGroupCalibrator:
camera_calibration_estimates: dict[CameraId, AniposeCameraCalibrationEstimate]
-
+
max_iterations: int = 10
starting_error_threshold: float = 15.0
ending_error_threshold: float = 1.0
@@ -335,79 +362,66 @@ class AniposeCameraGroupCalibrator:
def run_iterative_bundle_adjustment(
self,
- points2d: np.ndarray,
- extra_data: dict[str, Any],
+ calibration_input_data: CalibrationInputData,
) -> float:
- """Performs iterative bundle adjustment to refine camera parameters.
-
- Args:
- points2d (np.ndarray): A CxNx2 array of 2D points, where C is the number of cameras
- and N is the number of points.
- extra_data (Optional[Dict[str, Any]]): Optional dictionary for additional data associated
- with the points.
- max_iterations (int): Maximum number of iterations for the adjustment.
- starting_error_threshold (float): Initial value for the error threshold.
- ending_error_threshold (float): Final value for the error threshold.
- max_number_function_evals (int): Maximum number of function evaluations in bundle adjustment.
- function_tolerance (float): Tolerance for function convergence in bundle adjustment.
- number_of_samples_per_iteration (int): Number of points to sample per iteration.
- number_of_samples_full (int): Number of points for full sampling.
- error_threshold (float): Error threshold for early stopping.
- verbose (bool): If True, prints detailed logs during execution.
-
- Returns:
- float: Final mean reprojection error after adjustments.
"""
+ Performs iterative bundle adjustment to refine camera parameters.
+ based on Anipose's iterative bundle adjustment method. (`bundle_adjust_iter`)
+ """
+ original_calibration_input_data = deepcopy(calibration_input_data)
+ original_points2d = original_calibration_input_data.pixel_points2d
+
# Validate input shapes
- assert points2d.ndim == 3 and points2d.shape[2] == 2, "points2d must be a CxNx2 array"
- assert points2d.shape[0] == len(self.camera_calibration_estimates), (
- f"Invalid points shape, expected first dimension to be equal to "
- f"number of cameras ({len(self.camera_calibration_estimates)}), but got shape {points2d.shape}"
- )
+ if not original_points2d.shape[0] == len(self.camera_calibration_estimates):
+ raise ValueError(
+ f"Invalid points shape, expected first dimension to be equal to "
+ f"number of cameras ({len(self.camera_calibration_estimates)}), but got shape {original_points2d.shape}"
+ )
# Initialize
error_list = []
- original_points2d = points2d
- original_extra_data = extra_data
# log original error before resampling
- original_error = self.get_mean_reprojection_error(points2d, median=True)
+ original_error = self.get_mean_reprojection_error(original_points2d, median=True)
logger.info(
- f"Starting iterative bundle adjustment...\n Number of cameras: {len(self.camera_calibration_estimates)}\nNumber of points: {points2d.shape[1]}\nOriginal error: {original_error:.2f}")
+ f"Starting iterative bundle adjustment..."
+ f"\n\tNumber of cameras: {len(self.camera_calibration_estimates)}"
+ f"\n\tNumber of points: {original_points2d.shape[1]}"
+ f"\n\tOriginal error: {original_error:.2f}")
# Resample initial set of points to prioritize shared views
- points2d, extra_data = resample_points_based_on_shared_views(
- original_points2d,
- original_extra_data,
+ resampled_calibration_input_data = resample_points_based_on_shared_views(
+ original_calibration_input_data,
number_of_samples_to_return=self.number_of_samples_full
)
- initial_error = self.get_mean_reprojection_error(points2d, median=True)
+ initial_error = self.get_mean_reprojection_error(resampled_calibration_input_data.pixel_points2d, median=True)
logger.debug("Initial error after resampling: ", initial_error)
# Calculate dynamic error thresholds for each iteration
error_thresholds = np.exp(
- np.linspace(np.log(self.starting_error_threshold), np.log(self.ending_error_threshold), num=self.max_iterations))
+ np.linspace(np.log(self.starting_error_threshold), np.log(self.ending_error_threshold),
+ num=self.max_iterations))
logger.debug(f"Error thresholds we will use for each bundle adjustment iteration: {error_thresholds}")
# Iterative bundle adjustment
dynamic_error_threshold = self.starting_error_threshold
for iteration in range(self.max_iterations):
- resampled_points2d, resampled_extra_data = resample_points_based_on_shared_views(
- original_points2d, original_extra_data, number_of_samples_to_return=self.number_of_samples_full
+ resampled_calibration_input_data = resample_points_based_on_shared_views(
+ original_calibration_input_data, number_of_samples_to_return=self.number_of_samples_full
)
# Triangulate 3D points from resampled 2D points
- triangulated_points3d = self.triangulate(resampled_points2d)
+ triangulated_points3d = self.triangulate(resampled_calibration_input_data.pixel_points2d)
# Calculate reprojection errors
errors_full = self.calculate_reprojection_error(
- triangulated_points3d, resampled_points2d, mean=False
+ triangulated_points3d, resampled_calibration_input_data.pixel_points2d, mean=False
)
normalized_errors = self.calculate_reprojection_error(
- triangulated_points3d, resampled_points2d, mean=True
+ triangulated_points3d, resampled_calibration_input_data.pixel_points2d, mean=True
)
# Compute error statistics
@@ -417,17 +431,14 @@ class AniposeCameraGroupCalibrator:
# Determine good points based on current threshold
good_points_mask = normalized_errors < dynamic_error_threshold
- filtered_extra_data = subset_extra(resampled_extra_data, good_points_mask)
-
+ good_points_indicies = np.where(good_points_mask)[0]
# Update error list and check for convergence
current_error = np.median(normalized_errors)
error_list.append(current_error)
if current_error < self.error_threshold:
-
logger.debug(f"Converged after {iteration + 1} iterations")
break
-
logger.debug(f"Bundle Adjust Iteration {iteration + 1} -"
f"\n\tError: {current_error:.2f}, "
f"\n\tThreshold: {dynamic_error_threshold:.1f}, "
@@ -436,23 +447,22 @@ class AniposeCameraGroupCalibrator:
# Perform bundle adjustment
self.bundle_adjust_points2d(
- resampled_points2d[:, good_points_mask],
- filtered_extra_data,
+ resampled_calibration_input_data.resample_indicies(list(good_points_indicies)),
loss="linear",
function_tolerance=self.function_tolerance,
maximum_number_function_evals=self.max_number_function_evals,
)
# Final adjustments and error calculation
- final_resampled_points2d, final_resampled_extra_data = resample_points_based_on_shared_views(
- original_points2d, original_extra_data, number_of_samples_to_return=self.number_of_samples_full
+ final_calibration_input_data = resample_points_based_on_shared_views(
+ calibration_input_data=original_calibration_input_data,
+ number_of_samples_to_return=self.number_of_samples_full
)
- self.final_adjustments(points2d=final_resampled_points2d,
- extra_data=final_resampled_extra_data,
- error_threshold=dynamic_error_threshold,
-)
+ self.final_adjustments(calibration_input_data=final_calibration_input_data,
+ error_threshold=dynamic_error_threshold
+ )
- final_error = self.get_mean_reprojection_error(final_resampled_points2d, median=True)
+ final_error = self.get_mean_reprojection_error(final_calibration_input_data.pixel_points2d, median=True)
print("Final error: ", final_error)
return float(final_error)
@@ -477,10 +487,12 @@ class AniposeCameraGroupCalibrator:
f" number of cameras ({len(self.camera_calibration_estimates)}), but shape is {points_2d.shape}"
)
if not points_3d.shape[1] == 3:
- raise ValueError(f"Invalid points shape, second dim of 3D points should be 3, but shape is {points_3d.shape}")
+ raise ValueError(
+ f"Invalid points shape, second dim of 3D points should be 3, but shape is {points_3d.shape}")
if not points_2d.shape[2] == 2:
- raise ValueError(f"Invalid points shape, third dim of 2D points should be 2, but shape is {points_2d.shape}")
+ raise ValueError(
+ f"Invalid points shape, third dim of 2D points should be 2, but shape is {points_2d.shape}")
one_point = False
if len(points_3d.shape) == 1 and len(points_2d.shape) == 2:
@@ -489,12 +501,13 @@ class AniposeCameraGroupCalibrator:
one_point = True
n_cams, n_points, _ = points_2d.shape
- if not points_3d.shape == (n_points, 3, ):
- raise ValueError("shapes of 2D and 3D points are not consistent: " "2D={}, 3D={}".format(points_2d.shape, points_3d.shape))
+ if not points_3d.shape == (n_points, 3,):
+ raise ValueError("shapes of 2D and 3D points are not consistent: " "2D={}, 3D={}".format(points_2d.shape,
+ points_3d.shape))
errors = np.empty((n_cams, n_points, 2))
- for camera_number, cam in enumerate(self.camera_calibration_estimates):
+ for camera_number, cam in enumerate(self.camera_calibration_estimates.values()):
errors[camera_number] = cam.single_camera_reprojection_error(points_3d, points_2d[camera_number])
if mean:
@@ -513,7 +526,6 @@ class AniposeCameraGroupCalibrator:
return errors
-
def project_3d_to_2d(self, points3d: np.ndarray):
"""Given an Nx3 array of points, this returns an CxNx2 array of 2D points,
where C is the number of cameras"""
@@ -522,7 +534,7 @@ class AniposeCameraGroupCalibrator:
n_cams = len(self.camera_calibration_estimates)
projected_points2d = np.empty((n_cams, n_points, 2), dtype="float64")
- for camera_number, camera in enumerate(self.camera_calibration_estimates):
+ for camera_number, camera in enumerate(self.camera_calibration_estimates.values()):
projected_points2d[camera_number] = camera.project_3d_to_2d(points3d).reshape(n_points, 2)
# check shape
@@ -548,7 +560,7 @@ class AniposeCameraGroupCalibrator:
if undistort:
new_points = np.empty(points2d.shape)
- for camera_number, camera in enumerate(self.camera_calibration_estimates):
+ for camera_number, camera in enumerate(self.camera_calibration_estimates.values()):
# must copy in order to satisfy opencv underneath
sub = np.copy(points2d[camera_number])
new_points[camera_number] = camera.undistort_points(sub)
@@ -559,7 +571,7 @@ class AniposeCameraGroupCalibrator:
triangulated_points3d = np.empty((n_points, 3))
triangulated_points3d[:] = np.nan
- camera_matricies = np.array([camera.get_extrinsics_mat() for camera in self.camera_calibration_estimates])
+ camera_matricies = np.array([camera.extrinsics_matrix for camera in self.camera_calibration_estimates.values()])
if progress:
points_iterator = trange(n_points, ncols=70)
@@ -573,36 +585,21 @@ class AniposeCameraGroupCalibrator:
triangulated_points3d[point_index] = anipose_triangulate_simple(point_xy[point_xy_no_nans],
camera_matricies[point_xy_no_nans])
-
if one_point:
triangulated_points3d = triangulated_points3d[0]
return triangulated_points3d
-
-
def final_adjustments(self,
- points2d: np.ndarray,
- extra_data: dict[str, Any],
- error_threshold: float,
- function_tolerance: float,
- max_number_function_evals: int) -> None:
- """Perform final adjustments and bundle adjustment with a potentially updated threshold.
-
- Args:
- points2d (np.ndarray): Resampled 2D points.
- extra_data (Dict[str, Any]): Resampled extra data.
- error_threshold (float): Error threshold for filtering good points.
- function_tolerance (float): Tolerance for function convergence in bundle adjustment.
- max_number_function_evals (int): Maximum number of function evaluations in bundle adjustment.
- verbose (bool): If True, prints detailed logs during execution.
- """
- triangulated_points3d = self.triangulate(points2d)
+ calibration_input_data: CalibrationInputData,
+ error_threshold: float) -> None:
+ """Perform final adjustments and bundle adjustment with a potentially updated threshold. """
+ triangulated_points3d = self.triangulate(calibration_input_data.pixel_points2d)
errors_full = self.calculate_reprojection_error(
- triangulated_points3d, points2d, mean=False
+ triangulated_points3d, calibration_input_data.pixel_points2d, mean=False
)
normalized_errors = self.calculate_reprojection_error(
- triangulated_points3d, points2d, mean=True
+ triangulated_points3d, calibration_input_data.pixel_points2d, mean=True
)
error_dict = get_error_dict(errors_full)
@@ -611,61 +608,38 @@ class AniposeCameraGroupCalibrator:
max_error, min_error = calculate_error_bounds(error_dict)
dynamic_error_threshold = max(max(max_error, error_threshold), min_error)
good_points_mask = normalized_errors < dynamic_error_threshold
- filtered_extra_data = subset_extra(extra_data, good_points_mask)
-
+ good_points_indicies = np.where(good_points_mask)[0]
self.bundle_adjust_points2d(
- points2d[:, good_points_mask],
- filtered_extra_data,
+ calibration_input_data.resample_indicies(list(good_points_indicies)),
loss="linear",
- function_tolerance=function_tolerance,
- maximum_number_function_evals=max(200, max_number_function_evals),
+ function_tolerance=self.function_tolerance,
+ maximum_number_function_evals=self.max_number_function_evals,
)
def bundle_adjust_points2d(
self,
- points2d: np.ndarray,
- extra_data: dict[str, Any],
+ calibration_input_data: CalibrationInputData,
loss: str = "linear",
threshold: float = 50.0,
function_tolerance: float = 1e-4,
maximum_number_function_evals: int = 1000,
- weights: np.ndarray|None = None,
- start_params: np.ndarray|None = None,
+ weights: np.ndarray | None = None,
+ start_params: np.ndarray | None = None,
) -> float:
- """Perform bundle adjustment to fine-tune camera parameters.
-
- Args:
- points2d (np.ndarray): A CxNx2 array of 2D points, where C is the number of cameras
- and N is the number of points.
- extra_data (Optional[Dict[str, Any]]): Optional dictionary containing additional data
- such as 'ids'.
- loss (str): Loss function to use in optimization. Default is 'linear'.
- threshold (float): Scaling factor for the residuals.
- function_tolerance (float): Tolerance for function convergence in optimization.
- maximum_number_function_evals (int): Maximum number of function evaluations.
- weights (Optional[np.ndarray]): Optional weights for the points.
- start_params (Optional[np.ndarray]): Optional initial parameter estimates.
- verbose (bool): If True, prints detailed logs during execution.
-
- Returns:
- float: Final mean reprojection error after adjustments.
- """
+ """Perform bundle adjustment to fine-tune camera parameters"""
+ points2d = calibration_input_data.pixel_points2d
# Validate input shapes
if not points2d.ndim == 3 and points2d.shape[2] == 2:
raise ValueError("points2d must be a CxNx2 array")
- if not points2d.shape[0] == len(self.camera_calibration_estimates):
+ if not points2d.shape[0] == len(self.camera_calibration_estimates):
raise ValueError(
f"Invalid points shape, expected first dimension to be equal to "
f"number of cameras ({len(self.camera_calibration_estimates)}), but got shape {points2d.shape}"
)
- # Update extra_data data if provided, specifically remapping IDs
- if extra_data is not None and 'ids' in extra_data:
- extra_data["ids_map"] = remap_ids(extra_data["ids"])
-
# Initialize parameters for bundle adjustment
- initial_params, num_camera_params = self._initalize_bundle_adjust_parameters(points2d, extra_data)
+ initial_params, num_camera_params = self._initalize_bundle_adjust_parameters(calibration_input_data)
# Override initial parameters if start_params are provided
if start_params is not None:
@@ -674,7 +648,7 @@ class AniposeCameraGroupCalibrator:
# Define error function and sparsity pattern for optimization
- jac_sparsity = self._calculate_bundle_adjustment_jacobian_sparsity(points2d, num_camera_params, extra_data)
+ jac_sparsity = self._calculate_bundle_adjustment_jacobian_sparsity(calibration_input_data, num_camera_params)
# Set up optimization options
optimization_result = optimize.least_squares(
@@ -687,17 +661,17 @@ class AniposeCameraGroupCalibrator:
ftol=function_tolerance,
method="trf",
tr_solver="lsmr",
- verbose=2 ,
+ verbose=2,
max_nfev=maximum_number_function_evals,
- args=(points2d, num_camera_params, extra_data),
+ args=(points2d, num_camera_params),
)
# Update camera parameters with optimized results
optimized_params = optimization_result.x
- for i, camera in enumerate(self.camera_calibration_estimates):
+ for i, camera in enumerate(self.camera_calibration_estimates.values()):
start_idx = i * num_camera_params
end_idx = (i + 1) * num_camera_params
- camera.set_params(optimized_params[start_idx:end_idx])
+ camera.set_optimized_parameters(optimized_params[start_idx:end_idx])
# Calculate and return the mean reprojection error
mean_error = self.get_mean_reprojection_error(points2d)
@@ -705,19 +679,20 @@ class AniposeCameraGroupCalibrator:
@jit(parallel=True, forceobj=True)
def _bundle_adjust_error_function(self,
- optimizable_parameters:np.ndarray,
- points2d,
- number_of_camera_parameters,
- extra):
+ optimizable_parameters: np.ndarray,
+ calibration_input_data: CalibrationInputData,
+ number_of_camera_parameters: int,
+ ):
"""Error function for bundle adjustment"""
+ points2d = calibration_input_data.pixel_points2d
good = ~np.isnan(points2d)
n_cams = len(self.camera_calibration_estimates)
- for i in range(n_cams):
+ for i, cam in enumerate(self.camera_calibration_estimates.values()):
cam = self.camera_calibration_estimates[i]
a = i * number_of_camera_parameters
b = (i + 1) * number_of_camera_parameters
- cam.set_params(optimizable_parameters[a:b])
+ cam.set_optimized_parameters(optimizable_parameters[a:b])
n_cams = len(self.camera_calibration_estimates)
sub = number_of_camera_parameters * n_cams
@@ -726,27 +701,25 @@ class AniposeCameraGroupCalibrator:
errors = self.calculate_reprojection_error(p3ds_test, points2d)
errors_reproj = errors[good]
- ids = extra["ids_map"]
- objp = extra["objp"]
- min_scale = np.min(objp[objp > 0])
+ ids = remap_ids(calibration_input_data.ids)
+ object_points3d = calibration_input_data.object_points3d
+ min_scale = np.min(object_points3d[object_points3d > 0])
n_boards = int(np.max(ids)) + 1
a = sub + n3d
rotation_vectors = optimizable_parameters[a: a + n_boards * 3].reshape(-1, 3)
translation_vectors = optimizable_parameters[a + n_boards * 3: a + n_boards * 6].reshape(-1, 3)
- expected = transform_points(objp, rotation_vectors[ids], translation_vectors[ids])
+ expected = transform_points(object_points3d, rotation_vectors[ids], translation_vectors[ids])
errors_obj = 2 * (p3ds_test - expected).ravel() / min_scale
-
return np.hstack([errors_reproj, errors_obj])
def _calculate_bundle_adjustment_jacobian_sparsity(self,
- points2d: np.ndarray,
- number_of_camear_parameters: int,
- extra_data: dict[str, Any]) -> dok_matrix:
+ calibration_input_data: CalibrationInputData,
+ number_of_camear_parameters: int) -> dok_matrix:
"""Given an CxNx2 array of 2D points,
where N is the number of points and C is the number of cameras,
compute the sparsity structure of the jacobian for bundle adjustment"""
-
+ points2d = calibration_input_data.pixel_points2d
point_indices = np.zeros(points2d.shape, dtype="int32")
cam_indices = np.zeros(points2d.shape, dtype="int32")
@@ -758,8 +731,7 @@ class AniposeCameraGroupCalibrator:
good = ~np.isnan(points2d)
-
- ids = extra_data["ids_map"]
+ ids = remap_ids(calibration_input_data.ids)
n_boards = int(np.max(ids)) + 1
total_board_params = n_boards * (3 + 3) # rotation_vectors + translation_vectors
@@ -772,7 +744,6 @@ class AniposeCameraGroupCalibrator:
n_errors = n_good_values + n_points * 3
-
A_sparse = dok_matrix((n_errors, n_params), dtype="int16")
cam_indices_good = cam_indices[good]
@@ -818,24 +789,24 @@ class AniposeCameraGroupCalibrator:
return A_sparse
- def _initalize_bundle_adjust_parameters(self, points2d:np.ndarray, extra_data:dict[str, Any]):
+ def _initalize_bundle_adjust_parameters(self, calibration_input_data:CalibrationInputData):
"""Given an CxNx2 array of 2D points,
where N is the number of points and C is the number of cameras,
initializes the parameters for bundle adjustment"""
- cam_params = np.hstack([cam.get_opitmizad_parameters() for cam in self.camera_calibration_estimates])
+ cam_params = np.hstack([cam.get_optimizable_parameters() for cam in self.camera_calibration_estimates.values()])
n_cam_params = len(cam_params) // len(self.camera_calibration_estimates)
total_cam_params = len(cam_params)
- n_cams, n_points, _ = points2d.shape
+ n_cams, n_points, _ = calibration_input_data.pixel_points2d.shape
assert n_cams == len(self.camera_calibration_estimates), (
"number of cameras in CameraGroup does not " "match number of cameras in 2D points given"
)
- p3ds = self.triangulate(points2d)
+ points3d = self.triangulate(calibration_input_data.pixel_points2d)
- ids = extra_data["ids_map"]
+ ids = remap_ids(calibration_input_data.ids)
n_boards = int(np.max(ids[~np.isnan(ids)])) + 1
total_board_params = n_boards * (3 + 3) # rotation_vectors + translation_vectors
@@ -843,32 +814,32 @@ class AniposeCameraGroupCalibrator:
rotation_vectors = np.zeros((n_boards, 3), dtype="float64")
translation_vectors = np.zeros((n_boards, 3), dtype="float64")
- if "rotation_vectors" in extra_data and "translation_vectors" in extra_data:
- rotation_vectors_all = extra_data["rotation_vectors"]
- translation_vectors_all = extra_data["translation_vectors"]
- for board_num in range(n_boards):
- point_id = np.where(ids == board_num)[0][0]
- cam_ids_possible = np.where(~np.isnan(points2d[:, point_id, 0]))[0]
- cam_id = np.random.choice(cam_ids_possible)
- M_cam = self.camera_calibration_estimates[cam_id].extrinsics_matrix
- M_board_cam = make_M(rotation_vectors_all[cam_id, point_id], translation_vectors_all[cam_id, point_id])
- M_board = np.matmul(inverse(M_cam), M_board_cam)
- rvec, tvec = get_rtvec(M_board)
- rotation_vectors[board_num] = rvec
- translation_vectors[board_num] = tvec
-
- x0 = np.zeros(total_cam_params + p3ds.size + total_board_params)
+
+ rotation_vectors_all = calibration_input_data.rotation_vectors
+ translation_vectors_all = calibration_input_data.translation_vectors
+ for board_num in range(n_boards):
+ point_id = np.where(ids == board_num)[0][0]
+ cam_ids_possible = np.where(~np.isnan(calibration_input_data.pixel_points2d[:, point_id, 0]))[0]
+ cam_id = np.random.choice(cam_ids_possible)
+ camera_extrinsics_matrix = self.camera_calibration_estimates[cam_id].extrinsics_matrix
+ M_board_cam = construct_camera_extrinsics_matrix(rotation_vectors_all[cam_id, point_id],
+ translation_vectors_all[cam_id, point_id])
+ M_board = np.matmul(inverse(camera_extrinsics_matrix), M_board_cam)
+ rotation_vector, translation_vector = get_rotation_and_translation_vector_from_extrinsics_matrix(
+ M_board)
+ rotation_vectors[board_num] = rotation_vector
+ translation_vectors[board_num] = translation_vector
+
+ x0 = np.zeros(total_cam_params + points3d.size + total_board_params)
x0[:total_cam_params] = cam_params
- x0[total_cam_params: total_cam_params + p3ds.size] = p3ds.ravel()
+ x0[total_cam_params: total_cam_params + points3d.size] = points3d.ravel()
- if extra_data is not None:
- start_board = total_cam_params + p3ds.size
- x0[start_board: start_board + n_boards * 3] = rotation_vectors.ravel()
- x0[start_board + n_boards * 3: start_board + n_boards * 6] = translation_vectors.ravel()
+ start_board = total_cam_params + points3d.size
+ x0[start_board: start_board + n_boards * 3] = rotation_vectors.ravel()
+ x0[start_board + n_boards * 3: start_board + n_boards * 6] = translation_vectors.ravel()
return x0, n_cam_params
-
def get_mean_reprojection_error(self, points2d, median=False):
triangulated_points3d = self.triangulate(points2d)
errors = self.calculate_reprojection_error(triangulated_points3d, points2d, mean=True)