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.pyindex 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 deepcopyfrom dataclasses import dataclassfrom typing import Any@@ -37,13 +38,61 @@ def anipose_triangulate_simple(points2d: np.ndarray, camera_matrices: np.ndarrayA[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 lolpoints3d_xyzh = vh[-1]points3d_xyzh /= points3d_xyzh[3] # Normalize the last element to 1return 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[sreturn extra_datareturn {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_points2dn_cams = points2d.shape[0]points2d_valid = ~np.isnan(points2d[:, :, 0]) # Boolean array indicating valid (non-NaN) pointspoint_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 orderingfinal_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, tvecdef 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+@dataclassclass AniposeCameraCalibrationEstimate:name: strcamera_id: CameraIdsize: 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.ndarraycamera_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] = 1return 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-@dataclassclass AniposeCameraGroupCalibrator:camera_calibration_estimates: dict[CameraId, AniposeCameraCalibrationEstimate]-+max_iterations: int = 10starting_error_threshold: float = 15.0ending_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}"+ )# Initializeerror_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 iterationerror_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 adjustmentdynamic_error_threshold = self.starting_error_thresholdfor 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 errorserrors_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 thresholdgood_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 convergencecurrent_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 adjustmentself.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 = Falseif len(points_3d.shape) == 1 and len(points_2d.shape) == 2:@@ -489,12 +501,13 @@ class AniposeCameraGroupCalibrator:one_point = Truen_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 underneathsub = 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 shapesif 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 providedif 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 optionsoptimization_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 resultsoptimized_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_paramsend_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 errormean_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_points2dgood = ~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_parametersb = (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)) + 1a = sub + n3drotation_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_points2dpoint_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)) + 1total_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.shapeassert 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)])) + 1total_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)