Free Motion Capture for Everyone πβ¨
TypeScript
still chunkin
jonmatthisa5764fab8515
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 16a56870..2242cb4a 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,167 +2,341 @@# ore info on Anipoise: https://anipose.readthedocs.io/en/latest/import logging-import multiprocessing+from dataclasses import dataclassfrom typing import Any+import cv2import numpy as np-from freemocap.core_processes.capture_volume_calibration.anipose_camera_calibration.anipose_stuff.anipose_camera import \- AniposeCamera-from freemocap.core_processes.capture_volume_calibration.anipose_camera_calibration.anipose_stuff.anipose_functions import \- get_error_dict, subset_extra, resample_points_based_on_shared_views, remap_ids, \- transform_points, make_M, get_rtvec, calculate_error_bounds-from freemocap.core_processes.capture_volume_calibration.anipose_camera_calibration.anipose_stuff.anipose_triangulate_simple import \- anipose_triangulate_simplefrom numba import jitfrom scipy import optimizefrom scipy.linalg import inv as inversefrom scipy.sparse import dok_matrix+from skellycam import CameraIdfrom tqdm import trangelogger = logging.getLogger(__name__)-class AniposeCameraGroup:- def __init__(self, cameras: list[AniposeCamera], metadata: dict[str, Any]):- self.cameras = cameras- self.metadata = metadata-- 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"""- points3d = points3d.reshape(-1, 1, 3)- n_points = points3d.shape[0]- n_cams = len(self.cameras)-- projected_points2d = np.empty((n_cams, n_points, 2), dtype="float64")- for camera_number, camera in enumerate(self.cameras):- projected_points2d[camera_number] = camera.project_3d_to_2d(points3d).reshape(n_points, 2)-- # check shape- assert projected_points2d.shape == (n_cams, n_points,- 2), f"Invalid projected_points2d shape, should be {n_cams, n_points, 2}, but shape is {projected_points2d.shape}"-- return projected_points2d-- def triangulate(self, points2d, undistort=True, progress=False, kill_event: multiprocessing.Event = None):- """Given an CxNx2 array, this returns an Nx3 array of points,- where N is the number of points and C is the number of cameras"""-- assert points2d.shape[0] == len(- self.cameras- ), "Invalid points shape, first dim should be equal to" " number of cameras ({}), but shape is {}".format(- len(self.cameras), points2d.shape- )-- one_point = False- if len(points2d.shape) == 2:- points2d = points2d.reshape(-1, 1, 2)- one_point = True-- if undistort:- new_points = np.empty(points2d.shape)- for camera_number, camera in enumerate(self.cameras):- # must copy in order to satisfy opencv underneath- sub = np.copy(points2d[camera_number])- new_points[camera_number] = camera.undistort_points(sub)- points2d = new_points+@jit(nopython=True, parallel=False)+def anipose_triangulate_simple(points2d: np.ndarray, camera_matrices: np.ndarray) -> np.ndarray:+ """+ Triangulate 3D point from 2D points and camera matrices using the Direct Linear Transform (DLT) method.++ :param points2d: A 2D array of shape (N, 2), where N is the number of cameras. Each row represents the+ 2D coordinates (x, y) of the point in the image plane of each camera.+ :param camera_matrices: A 3D array of shape (N, 3, 4), where each element is a 3x4 camera projection matrix.+ :return: A 1D array of shape (3,) representing the triangulated 3D point.+ """++ num_cams = camera_matrices.shape[0]+ A = np.zeros((num_cams * 2, 4))++ for i in range(num_cams):+ x, y = points2d[i]+ mat = camera_matrices[i]+ 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++ points3d_xyzh = vh[-1]+ points3d_xyzh /= points3d_xyzh[3] # Normalize the last element to 1++ return points3d_xyzh[:3]++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.++ Args:+ error_dict (Dict[Tuple[int, int], Tuple[int, np.ndarray]]): Dictionary containing error statistics+ for camera pairs, where the key is a tuple of camera indices and the value is a tuple of+ (number of observations, percentiles).++ Returns:+ Tuple[float, float]: A tuple of (max_error, min_error) based on the percentiles of error means.+ """+ max_error = 0.0+ min_error = float('inf')+ for _, (_, percents) in error_dict.items():+ max_error = max(percents[-1], max_error)+ min_error = min(percents[0], min_error)+ return max_error, min_error+++def transform_points(points, rotation_vectors, translation_vectors):+ """Rotate points by given rotation vectors and translate.+ Rodrigues' rotation formula is used.+ """+ theta = np.linalg.norm(rotation_vectors, axis=1)[:, np.newaxis]+ with np.errstate(invalid="ignore"):+ v = rotation_vectors / theta+ v = np.nan_to_num(v)+ dot = np.sum(points * v, axis=1)[:, np.newaxis]+ cos_theta = np.cos(theta)+ sin_theta = np.sin(theta)++ rotated = cos_theta * points + sin_theta * np.cross(v, points) + dot * (1 - cos_theta) * v++ return rotated + translation_vectors++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.++ :param ids: An array-like structure containing the original identifiers.+ :return: A NumPy array with the identifiers remapped to a consecutive range.+ """+ ids_array = np.asarray(ids)+ unique_ids, remapped_ids = np.unique(ids_array, return_inverse=True)+ return remapped_ids+++def subset_extra_data(extra_data: dict[str, Any], indices: np.ndarray) -> dict[str, Any]:+ """+ Subset the extra data dictionary based on the selected indices.++ :param extra_data: The dictionary containing extra data.+ :param indices: The indices to subset the data.+ :return: A new dictionary with data subset according to the provided indices.+ """+ if not extra_data:+ 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]:+ """+ 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.+ """++ 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])+ num_cams = np.sum(points2d_valid, axis=0) # Number of cameras observing each point++ included_points = set()++ # Iterate over each unique pair of cameras+ for first_cam in range(n_cams):+ for second_cam in range(first_cam + 1, n_cams):+ # Determine which points are visible in both selected cameras+ visible_in_both = points2d_valid[first_cam] & points2d_valid[second_cam]+ n_good = np.sum(visible_in_both)++ if n_good > 0:+ # Select points seen by the most cameras, adding a small random value for tie-breaking+ visibility_scores = num_cams[visible_in_both].astype(np.float64)+ visibility_scores += np.random.random(size=visibility_scores.shape)++ # Pick the top n_samp points based on visibility scores+ picked_indices = np.argsort(-visibility_scores)[:number_of_samples_to_return]+ selected_points = point_indices[visible_in_both][picked_indices]++ # Add selected points to the set of included points+ included_points.update(selected_points)++ # 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 resampled_ponts, extra_data++def subset_extra(extra, ixs):+ if extra is None:+ return None++ 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 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]+ errors_norm = np.linalg.norm(errors_full, axis=2)++ good = ~np.isnan(errors_full[:, :, 0])++ error_dict = dict()++ for i in range(n_cams):+ for j in range(i + 1, n_cams):+ subset = good[i] & good[j]+ err_subset = errors_norm[:, subset][[i, j]]+ err_subset_mean = np.mean(err_subset, axis=0)+ if np.sum(subset) > min_points:+ percents = np.percentile(err_subset_mean, [15, 75])+ # percents = np.percentile(err_subset, [25, 75])+ 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++ camera_matrix: np.ndarray = np.eye(3, dtype="float64")+ distortion_coefficients: np.ndarray = np.zeros(5),++ @property+ def focal_length(self):+ fx = self.camera_matrix[0, 0]+ fy = self.camera_matrix[1, 1]+ return (fx + fy) / 2.0++ @focal_length.setter+ def focal_length(self, fx: float, fy: float = None):+ if fy is None:+ fy = fx+ self.camera_matrix[0, 0] = fx+ self.camera_matrix[1, 1] = fy++ @property+ def extrinsics_matrix(self) -> np.ndarray:+ """+ Returns the extrinsics matrix of the camera, which is a 4x4 matrix that transforms points from the camera coordinate system to the world coordinate system.+ the left upper 3x3 matrix is the rotation matrix, and the rightmost column is the translation vector.+ the bottom row is [0, 0, 0, 1] (where the 1 is the homogeneous coordinate, which makes the matrix invertible and provides the scale factor for the translation vector to put it in spatial coordinates)+ """- n_cams, n_points, _ = points2d.shape+ extrinsics_matrix = np.zeros((4, 4))- triangulated_points3d = np.empty((n_points, 3))- triangulated_points3d[:] = np.nan+ extrinsics_matrix[:3, :3] = cv2.Rodrigues(self.rotation_vector)+ extrinsics_matrix[:3, 3] = self.translation_vector.flatten()+ extrinsics_matrix[3, 3] = 1+ return extrinsics_matrix- camera_matricies = np.array([camera.get_extrinsics_mat() for camera in self.cameras])- if progress:- points_iterator = trange(n_points, ncols=70)- else:- points_iterator = range(n_points)- for point_index in points_iterator:- point_xy = points2d[:, point_index, :]- point_xy_no_nans = ~np.isnan(point_xy[:, 0])- if np.sum(point_xy_no_nans) >= 2:- triangulated_points3d[point_index] = anipose_triangulate_simple(point_xy[point_xy_no_nans],- camera_matricies[point_xy_no_nans])-- if kill_event is not None and kill_event.is_set():- return None-- if one_point:- triangulated_points3d = triangulated_points3d[0]-- return triangulated_points3d-- @jit(parallel=True, forceobj=True)- def calculate_reprojection_error(self,- points_3d: np.ndarray,- points_2d: np.ndarray,- mean=False):- """Given an Nx3 array of 3D points and an CxNx2 array of 2D points,- where N is the number of points and C is the number of cameras,- this returns an CxNx2 array of errors.- Optionally mean=True, this averages the errors and returns array of length N of errors"""- if not points_3d.shape[0] == points_2d.shape[1]:- raise ValueError(- f"Invalid points shape, first dim of 3D points should be equal to"- f" number of points in 2D points ({points_2d.shape[1]}), but shape is {points_3d.shape}"- )- if not points_2d.shape[0] == len(self.cameras):- raise ValueError(- f"Invalid points shape, first dim of 2D points should be equal to"- f" number of cameras ({len(self.cameras)}), 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}")+ def get_optimizable_parameters(self):+ """+ Grabs the parametesr that will be optimed and organizes them in a numpy array+ :return:+ """+ optimized_parameters = np.zeros(9, dtype="float64")+ optimized_parameters[0:3] = self.rotation_vector+ optimized_parameters[3:6] = self.translation_vector+ optimized_parameters[6] = self.focal_length+ optimized_parameters[7] = self.distortion_coefficients[0]+ optimized_parameters[8] = self.distortion_coefficients[1]+ return optimized_parameters++ def set_optimized_parameters(self, optimized_parameters: np.ndarray):+ """+ Sets camera parameters from optimized parameters- 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}")- one_point = False- if len(points_3d.shape) == 1 and len(points_2d.shape) == 2:- points_3d = points_3d.reshape(1, 3)- points_2d = points_2d.reshape(-1, 1, 2)- one_point = True+ :param optimized_parameters:+ :return:+ """- 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))+ self.rotation_vector = optimized_parameters[0:3]+ self.translation_vector = optimized_parameters[3:6]+ self.focal_length = optimized_parameters[6]- errors = np.empty((n_cams, n_points, 2))+ distortion_coefficients = np.zeros(5)+ distortion_coefficients[0] = optimized_parameters[7]+ distortion_coefficients[1] = optimized_parameters[8]+ self.distortion_coefficients = distortion_coefficients- for camera_number, cam in enumerate(self.cameras):- errors[camera_number] = cam.single_camera_reprojection_error(points_3d, points_2d[camera_number])- if mean:- errors_norm = np.linalg.norm(errors, axis=2)- good = ~np.isnan(errors_norm)- errors_norm[~good] = 0- denom = np.sum(good, axis=0).astype("float64")- denom[denom < 1.5] = np.nan- errors = np.sum(errors_norm, axis=0) / denom+ def undistort_points(self, points2d: np.ndarray) -> np.ndarray:+ """+ Undistorts 2D points using the camera's intrinsic parameters and distortion coefficients.- if one_point:- if mean:- errors = float(errors[0])- else:- errors = errors.reshape(-1, 2)+ This function adjusts the provided 2D points to account for lens distortion, returning+ points that are mapped as they would appear in an ideal pinhole camera model. The process+ ensures that the ray passing from the nodal point of the camera lens through the+ undistorted 2D point intersects the corresponding 3D point in space accurately.- return errors+ :param points2d: A NumPy array of shape (N, 2) or (N, M, 2) representing the 2D points+ to be undistorted. These are typically the distorted pixel coordinates+ captured by the camera.+ :return: A NumPy array of the same shape as `points2d` containing the undistorted 2D points.+ """+ shape = points2d.shape+ points2d = points2d.reshape(-1, 1, 2)+ undistorted_points2d = cv2.undistortPoints(points2d, self.camera_matrix.astype("float64"),+ self.distortion_coefficients.astype("float64"))+ return undistorted_points2d.reshape(shape)++ def project_3d_to_2d(self, points3d: np.ndarray) -> np.ndarray:+ if not points3d.shape[-1] == 3:+ raise ValueError("points3d must have shape (N, 3)")+ points3d = points3d.reshape(-1, 1, 3)+ projected_2d_points, _ = cv2.projectPoints(+ points3d,+ self.rotation_vector,+ self.translation_vector,+ self.camera_matrix.astype("float64"),+ self.distortion_coefficients.astype("float64"),+ )+ if projected_2d_points.shape != (points3d.shape[0], 1, 2):+ raise ValueError(f"projected_2d_points has incorrect shape: {projected_2d_points.shape}")++ return projected_2d_points++ 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)?+ 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+ max_number_function_evals: int = 200+ function_tolerance: float = 1e-4+ number_of_samples_per_iteration: int = 100+ number_of_samples_full: int = 1000+ error_threshold: float = 0.3def run_iterative_bundle_adjustment(self,points2d: np.ndarray,- extra_data: dict[str, Any] | None = None,- max_iterations: int = 10,- starting_error_threshold: float = 15.0,- ending_error_threshold: float = 1.0,- max_number_function_evals: int = 200,- function_tolerance: float = 1e-4,- number_of_samples_per_iteration: int = 100,- number_of_samples_full: int = 1000,- error_threshold: float = 0.3,- verbose: bool = False,+ extra_data: dict[str, Any],+) -> float:"""Performs iterative bundle adjustment to refine camera parameters.@@ -186,9 +360,9 @@ class AniposeCameraGroup:"""# Validate input shapesassert points2d.ndim == 3 and points2d.shape[2] == 2, "points2d must be a CxNx2 array"- assert points2d.shape[0] == len(self.cameras), (+ 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.cameras)}), but got shape {points2d.shape}"+ f"number of cameras ({len(self.camera_calibration_estimates)}), but got shape {points2d.shape}")# Initialize@@ -196,10 +370,10 @@ class AniposeCameraGroup:original_points2d = points2doriginal_extra_data = extra_data- if verbose:- original_error = self.get_mean_reprojection_error(points2d, median=True)- logger.info(- f"Starting iterative bundle adjustment...\n Number of cameras: {len(self.cameras)}\nNumber of points: {points2d.shape[1]}\nOriginal error: {original_error:.2f}")+ # log original error before resampling+ original_error = self.get_mean_reprojection_error(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}")# Resample initial set of points to prioritize shared viewspoints2d, extra_data = resample_points_based_on_shared_views(@@ -208,14 +382,14 @@ class AniposeCameraGroup:number_of_samples_to_return=number_of_samples_full)initial_error = self.get_mean_reprojection_error(points2d, median=True)- if verbose:- logger.debug("Initial error after resampling: ", initial_error)++ logger.debug("Initial error after resampling: ", initial_error)# Calculate dynamic error thresholds for each iterationerror_thresholds = np.exp(np.linspace(np.log(starting_error_threshold), np.log(ending_error_threshold), num=max_iterations))- if verbose:- logger.debug(f"Error thresholds we will use for each bundle adjustment iteration: {error_thresholds}")++ logger.debug(f"Error thresholds we will use for each bundle adjustment iteration: {error_thresholds}")# Iterative bundle adjustmentdynamic_error_threshold = starting_error_threshold@@ -248,16 +422,16 @@ class AniposeCameraGroup:current_error = np.median(normalized_errors)error_list.append(current_error)if current_error < error_threshold:- if verbose:- logger.debug(f"Converged after {iteration + 1} iterations")++ logger.debug(f"Converged after {iteration + 1} iterations")break- if verbose:- logger.debug(f"Bundle Adjust Iteration {iteration + 1} -"- f"\n\tError: {current_error:.2f}, "- f"\n\tThreshold: {dynamic_error_threshold:.1f}, "- f"\n\tProportion of points that meet this threshold: {np.mean(good_points_mask):.3f}")- logger.debug("Error scores from prior iterations", error_list)++ logger.debug(f"Bundle Adjust Iteration {iteration + 1} -"+ f"\n\tError: {current_error:.2f}, "+ f"\n\tThreshold: {dynamic_error_threshold:.1f}, "+ f"\n\tProportion of points that meet this threshold: {np.mean(good_points_mask):.3f}")+ logger.debug("Error scores from prior iterations", error_list)# Perform bundle adjustmentself.bundle_adjust_points2d(@@ -266,7 +440,6 @@ class AniposeCameraGroup:loss="linear",function_tolerance=function_tolerance,maximum_number_function_evals=max_number_function_evals,- verbose=verbose,)# Final adjustments and error calculation@@ -277,15 +450,135 @@ class AniposeCameraGroup:extra_data=final_resampled_extra_data,error_threshold=dynamic_error_threshold,function_tolerance=function_tolerance,- max_number_function_evals=max_number_function_evals,- verbose=verbose)+ max_number_function_evals=max_number_function_evals)final_error = self.get_mean_reprojection_error(final_resampled_points2d, median=True)- if verbose:- print("Final error: ", final_error)+ print("Final error: ", final_error)return float(final_error)+ @jit(parallel=True, forceobj=True)+ def calculate_reprojection_error(self,+ points_3d: np.ndarray,+ points_2d: np.ndarray,+ mean=False):+ """Given an Nx3 array of 3D points and an CxNx2 array of 2D points,+ where N is the number of points and C is the number of cameras,+ this returns an CxNx2 array of errors.+ Optionally mean=True, this averages the errors and returns array of length N of errors"""+ if not points_3d.shape[0] == points_2d.shape[1]:+ raise ValueError(+ f"Invalid points shape, first dim of 3D points should be equal to"+ f" number of points in 2D points ({points_2d.shape[1]}), but shape is {points_3d.shape}"+ )+ if not points_2d.shape[0] == len(self.camera_calibration_estimates):+ raise ValueError(+ f"Invalid points shape, first dim of 2D points should be equal to"+ 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}")++ 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}")++ one_point = False+ if len(points_3d.shape) == 1 and len(points_2d.shape) == 2:+ points_3d = points_3d.reshape(1, 3)+ points_2d = points_2d.reshape(-1, 1, 2)+ 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))++ errors = np.empty((n_cams, n_points, 2))++ for camera_number, cam in enumerate(self.camera_calibration_estimates):+ errors[camera_number] = cam.single_camera_reprojection_error(points_3d, points_2d[camera_number])++ if mean:+ errors_norm = np.linalg.norm(errors, axis=2)+ good = ~np.isnan(errors_norm)+ errors_norm[~good] = 0+ denom = np.sum(good, axis=0).astype("float64")+ denom[denom < 1.5] = np.nan+ errors = np.sum(errors_norm, axis=0) / denom++ if one_point:+ if mean:+ errors = float(errors[0])+ else:+ errors = errors.reshape(-1, 2)++ 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"""+ points3d = points3d.reshape(-1, 1, 3)+ n_points = points3d.shape[0]+ 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):+ projected_points2d[camera_number] = camera.project_3d_to_2d(points3d).reshape(n_points, 2)++ # check shape+ assert projected_points2d.shape == (n_cams, n_points,+ 2), f"Invalid projected_points2d shape, should be {n_cams, n_points, 2}, but shape is {projected_points2d.shape}"++ return projected_points2d++ def triangulate(self, points2d, undistort=True, progress=False):+ """Given an CxNx2 array, this returns an Nx3 array of points,+ where N is the number of points and C is the number of cameras"""++ assert points2d.shape[0] == len(+ self.camera_calibration_estimates+ ), "Invalid points shape, first dim should be equal to" " number of cameras ({}), but shape is {}".format(+ len(self.camera_calibration_estimates), points2d.shape+ )++ one_point = False+ if len(points2d.shape) == 2:+ points2d = points2d.reshape(-1, 1, 2)+ one_point = True++ if undistort:+ new_points = np.empty(points2d.shape)+ for camera_number, camera in enumerate(self.camera_calibration_estimates):+ # must copy in order to satisfy opencv underneath+ sub = np.copy(points2d[camera_number])+ new_points[camera_number] = camera.undistort_points(sub)+ points2d = new_points++ n_cams, n_points, _ = points2d.shape++ 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])++ if progress:+ points_iterator = trange(n_points, ncols=70)+ else:+ points_iterator = range(n_points)++ for point_index in points_iterator:+ point_xy = points2d[:, point_index, :]+ point_xy_no_nans = ~np.isnan(point_xy[:, 0])+ if np.sum(point_xy_no_nans) >= 2:+ 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,@@ -293,8 +586,7 @@ class AniposeCameraGroup:extra_data: dict[str, Any],error_threshold: float,function_tolerance: float,- max_number_function_evals: int,- verbose: bool) -> None:+ max_number_function_evals: int) -> None:"""Perform final adjustments and bundle adjustment with a potentially updated threshold.Args:@@ -313,8 +605,8 @@ class AniposeCameraGroup:triangulated_points3d, points2d, mean=True)error_dict = get_error_dict(errors_full)- if verbose:- logger.debug("Error dictionary after final adjustments", error_dict)++ logger.debug("Error dictionary after final adjustments", error_dict)max_error, min_error = calculate_error_bounds(error_dict)dynamic_error_threshold = max(max(max_error, error_threshold), min_error)@@ -327,20 +619,19 @@ class AniposeCameraGroup:loss="linear",function_tolerance=function_tolerance,maximum_number_function_evals=max(200, max_number_function_evals),- verbose=verbose,)def bundle_adjust_points2d(self,points2d: np.ndarray,- extra_data: dict[str, Any] = None,+ extra_data: dict[str, Any],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,- verbose: bool = True,+) -> float:"""Perform bundle adjustment to fine-tune camera parameters.@@ -363,10 +654,10 @@ class AniposeCameraGroup:# 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.cameras):+ 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.cameras)}), but got shape {points2d.shape}"+ f"number of cameras ({len(self.camera_calibration_estimates)}), but got shape {points2d.shape}")# Update extra_data data if provided, specifically remapping IDs@@ -379,7 +670,7 @@ class AniposeCameraGroup:# Override initial parameters if start_params are providedif start_params is not None:initial_params = start_params- num_camera_params = len(self.cameras[0].get_params())+ num_camera_params = len(self.camera_calibration_estimates[0].get_optimizable_parameters())# Define error function and sparsity pattern for optimization@@ -396,14 +687,14 @@ class AniposeCameraGroup:ftol=function_tolerance,method="trf",tr_solver="lsmr",- verbose=2 * verbose,+ verbose=2 ,max_nfev=maximum_number_function_evals,args=(points2d, num_camera_params, extra_data),)# Update camera parameters with optimized resultsoptimized_params = optimization_result.x- for i, camera in enumerate(self.cameras):+ for i, camera in enumerate(self.camera_calibration_estimates):start_idx = i * num_camera_paramsend_idx = (i + 1) * num_camera_paramscamera.set_params(optimized_params[start_idx:end_idx])@@ -413,43 +704,45 @@ class AniposeCameraGroup:return float(mean_error)@jit(parallel=True, forceobj=True)- def _bundle_adjust_error_function(self, params, points2d, n_cam_params, extra):+ def _bundle_adjust_error_function(self,+ optimizable_parameters:np.ndarray,+ points2d,+ number_of_camera_parameters,+ extra):"""Error function for bundle adjustment"""good = ~np.isnan(points2d)- n_cams = len(self.cameras)+ n_cams = len(self.camera_calibration_estimates)for i in range(n_cams):- cam = self.cameras[i]- a = i * n_cam_params- b = (i + 1) * n_cam_params- cam.set_params(params[a:b])+ 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])- n_cams = len(self.cameras)- sub = n_cam_params * n_cams+ n_cams = len(self.camera_calibration_estimates)+ sub = number_of_camera_parameters * n_camsn3d = points2d.shape[1] * 3- p3ds_test = params[sub: sub + n3d].reshape(-1, 3)+ p3ds_test = optimizable_parameters[sub: sub + n3d].reshape(-1, 3)errors = self.calculate_reprojection_error(p3ds_test, points2d)errors_reproj = errors[good]- if extra is not None:- ids = extra["ids_map"]- objp = extra["objp"]- min_scale = np.min(objp[objp > 0])- n_boards = int(np.max(ids)) + 1- a = sub + n3d- rvecs = params[a: a + n_boards * 3].reshape(-1, 3)- tvecs = params[a + n_boards * 3: a + n_boards * 6].reshape(-1, 3)- expected = transform_points(objp, rvecs[ids], tvecs[ids])- errors_obj = 2 * (p3ds_test - expected).ravel() / min_scale- else:- errors_obj = np.array([])+ ids = extra["ids_map"]+ objp = extra["objp"]+ min_scale = np.min(objp[objp > 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])+ 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: dict[str, Any] | None = None) -> dok_matrix:+ extra_data: dict[str, Any]) -> 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"""@@ -465,13 +758,10 @@ class AniposeCameraGroup:good = ~np.isnan(points2d)- if extra is not None:- ids = extra["ids_map"]- n_boards = int(np.max(ids)) + 1- total_board_params = n_boards * (3 + 3) # rvecs + tvecs- else:- n_boards = 0- total_board_params = 0++ ids = extra_data["ids_map"]+ n_boards = int(np.max(ids)) + 1+ total_board_params = n_boards * (3 + 3) # rotation_vectors + translation_vectorsn_cams = points2d.shape[0]n_points = points2d.shape[1]@@ -479,10 +769,9 @@ class AniposeCameraGroup:n_params = total_params_reproj + total_board_paramsn_good_values = np.sum(good)- if extra is not None:- n_errors = n_good_values + n_points * 3- else:- n_errors = n_good_values++ n_errors = n_good_values + n_points * 3+A_sparse = dok_matrix((n_errors, n_params), dtype="int16")@@ -501,76 +790,72 @@ class AniposeCameraGroup:A_sparse[ix, n_cams * number_of_camear_parameters + point_indices_good * 3 + i] = 1# -- match for the object points--- if extra is not None:- point_ix = np.arange(n_points)-- ## update all the camera parameters- # A_sparse[n_good_values:n_good_values+n_points*3,- # 0:n_cams*n_cam_params] = 1-- ## update board rotation and translation based on error from expected- for i in range(3):- for j in range(3):- A_sparse[- n_good_values + point_ix * 3 + i,- total_params_reproj + ids * 3 + j,- ] = 1- A_sparse[- n_good_values + point_ix * 3 + i,- total_params_reproj + n_boards * 3 + ids * 3 + j,- ] = 1-- ## update point position based on error from expected- for i in range(3):++ point_ix = np.arange(n_points)++ ## update all the camera parameters+ # A_sparse[n_good_values:n_good_values+n_points*3,+ # 0:n_cams*n_cam_params] = 1++ ## update board rotation and translation based on error from expected+ for i in range(3):+ for j in range(3):A_sparse[n_good_values + point_ix * 3 + i,- n_cams * number_of_camear_parameters + point_ix * 3 + i,+ total_params_reproj + ids * 3 + j,] = 1+ A_sparse[+ n_good_values + point_ix * 3 + i,+ total_params_reproj + n_boards * 3 + ids * 3 + j,+ ] = 1++ ## update point position based on error from expected+ for i in range(3):+ A_sparse[+ n_good_values + point_ix * 3 + i,+ n_cams * number_of_camear_parameters + point_ix * 3 + i,+ ] = 1return A_sparse- def _initalize_bundle_adjust_parameters(self, points2d:np.ndarray, extra_data:dict[str, Any] | None):+ def _initalize_bundle_adjust_parameters(self, points2d:np.ndarray, extra_data:dict[str, Any]):"""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_params() for cam in self.cameras])- n_cam_params = len(cam_params) // len(self.cameras)+ cam_params = np.hstack([cam.get_opitmizad_parameters() for cam in self.camera_calibration_estimates])+ n_cam_params = len(cam_params) // len(self.camera_calibration_estimates)total_cam_params = len(cam_params)n_cams, n_points, _ = points2d.shape- assert n_cams == len(self.cameras), (+ 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)- if extra_data is not None:- ids = extra_data["ids_map"]- n_boards = int(np.max(ids[~np.isnan(ids)])) + 1- total_board_params = n_boards * (3 + 3) # rvecs + tvecs-- # initialize to 0- rvecs = np.zeros((n_boards, 3), dtype="float64")- tvecs = np.zeros((n_boards, 3), dtype="float64")-- if "rvecs" in extra_data and "tvecs" in extra_data:- rvecs_all = extra_data["rvecs"]- tvecs_all = extra_data["tvecs"]- 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.cameras[cam_id].get_extrinsics_mat()- M_board_cam = make_M(rvecs_all[cam_id, point_id], tvecs_all[cam_id, point_id])- M_board = np.matmul(inverse(M_cam), M_board_cam)- rvec, tvec = get_rtvec(M_board)- rvecs[board_num] = rvec- tvecs[board_num] = tvec-- else:- total_board_params = 0+ ids = extra_data["ids_map"]+ n_boards = int(np.max(ids[~np.isnan(ids)])) + 1+ total_board_params = n_boards * (3 + 3) # rotation_vectors + translation_vectors++ # initialize to 0+ 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] = tvecx0 = np.zeros(total_cam_params + p3ds.size + total_board_params)x0[:total_cam_params] = cam_params@@ -578,8 +863,8 @@ class AniposeCameraGroup:if extra_data is not None:start_board = total_cam_params + p3ds.size- x0[start_board: start_board + n_boards * 3] = rvecs.ravel()- x0[start_board + n_boards * 3: start_board + n_boards * 6] = tvecs.ravel()+ 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_paramsdiff --git a/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/anipose_stuff/anipose_camera.py b/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/anipose_stuff/anipose_camera.pydeleted file mode 100644index a367770c..00000000--- a/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/anipose_stuff/anipose_camera.py+++ /dev/null@@ -1,162 +0,0 @@-import cv2-import numpy as np-from aniposelib.utils import make_M---class AniposeCamera:- def __init__(- self,- matrix=np.eye(3),- dist=np.zeros(5),- size=None,- rvec=np.zeros(3),- tvec=np.zeros(3),- name=None,- extra_dist=False,- ):- self.set_camera_matrix(matrix)- self.set_distortions(dist)- self.set_size(size)- self.set_rotation(rvec)- self.set_translation(tvec)- self.set_name(name)- self.extra_dist = extra_dist-- def get_dict(self):- return {- "name": self.get_name(),- "size": list(self.get_size()),- "matrix": self.get_camera_matrix().tolist(),- "distortions": self.get_distortions().tolist(),- "rotation": self.get_rotation().tolist(),- "translation": self.get_translation().tolist(),- }-- def load_dict(self, d):- self.set_camera_matrix(d["matrix"])- self.set_rotation(d["rotation"])- self.set_translation(d["translation"])- self.set_distortions(d["distortions"])- self.set_name(d["name"])- self.set_size(d["size"])-- def from_dict(d):- cam = Camera()- cam.load_dict(d)- return cam-- def get_camera_matrix(self):- return self.camera_matrix-- def get_distortions(self):- return self.distortion_coefficients-- def set_camera_matrix(self, matrix):- self.camera_matrix = np.array(matrix, dtype="float64")-- def set_focal_length(self, fx, fy=None):- if fy is None:- fy = fx- self.camera_matrix[0, 0] = fx- self.camera_matrix[1, 1] = fy-- def get_focal_length(self, both=False):- fx = self.camera_matrix[0, 0]- fy = self.camera_matrix[1, 1]- if both:- return (fx, fy)- else:- return (fx + fy) / 2.0-- def set_distortions(self, dist):- self.distortion_coefficients = np.array(dist, dtype="float64").ravel()-- def set_rotation(self, rvec):- self.rotation_vector = np.array(rvec, dtype="float64").ravel()-- def get_rotation(self):- return self.rotation_vector-- def set_translation(self, tvec):- self.translation_vector = np.array(tvec, dtype="float64").ravel()-- def get_translation(self):- return self.translation_vector-- def get_extrinsics_mat(self):- return make_M(self.rotation_vector, self.translation_vector)-- def get_name(self):- return self.name-- def set_name(self, name):- self.name = str(name)-- def set_size(self, size):- """set size as (width, height)"""- self.size = size-- def get_size(self):- """get size as (width, height)"""- return self.size-- def resize_camera(self, scale):- """resize the camera by scale factor, updating intrinsics to match"""- size = self.get_size()- new_size = size[0] * scale, size[1] * scale- matrix = self.get_camera_matrix()- new_matrix = matrix * scale- new_matrix[2, 2] = 1- self.set_size(new_size)- self.set_camera_matrix(new_matrix)-- def get_params(self):- params = np.zeros(8 + self.extra_dist, dtype="float64")- params[0:3] = self.get_rotation()- params[3:6] = self.get_translation()- params[6] = self.get_focal_length()- dist = self.get_distortions()- params[7] = dist[0]- if self.extra_dist:- params[8] = dist[1]- return params-- def set_params(self, params):- self.set_rotation(params[0:3])- self.set_translation(params[3:6])- self.set_focal_length(params[6])-- dist = np.zeros(5, dtype="float64")- dist[0] = params[7]- if self.extra_dist:- dist[1] = params[8]- self.set_distortions(dist)--- def undistort_points(self, points):- shape = points.shape- points = points.reshape(-1, 1, 2)- out = cv2.undistortPoints(points, self.camera_matrix.astype("float64"), self.distortion_coefficients.astype("float64"))- return out.reshape(shape)-- def project_3d_to_2d(self, points3d:np.ndarray) -> np.ndarray:- if not points3d.shape[-1] == 3:- raise ValueError("points3d must have shape (N, 3)")- points3d = points3d.reshape(-1, 1, 3)- projected_2d_points, _ = cv2.projectPoints(- points3d,- self.rotation_vector,- self.translation_vector,- self.camera_matrix.astype("float64"),- self.distortion_coefficients.astype("float64"),- )- if projected_2d_points.shape != (points3d.shape[0], 1, 2):- raise ValueError(f"projected_2d_points has incorrect shape: {projected_2d_points.shape}")-- return projected_2d_points-- def single_camera_reprojection_error(self, p3d, p2d):- projecting_3d_points_onto_2d_image_plane_og = self.project(p3d)- projecting_3d_points_onto_2d_image_plane = projecting_3d_points_onto_2d_image_plane_og.reshape(p2d.shape)- return p2d - projecting_3d_points_onto_2d_image_plane-diff --git a/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/anipose_stuff/anipose_functions.py b/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/anipose_stuff/anipose_functions.pyindex 1909a644..cd1a16a8 100644--- a/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/anipose_stuff/anipose_functions.py+++ b/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/anipose_stuff/anipose_functions.py@@ -9,195 +9,15 @@ from scipy.cluster.hierarchy import linkage, fclusterfrom scipy.cluster.vq import whitenfrom scipy.linalg import inv as inverse-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]- errors_norm = np.linalg.norm(errors_full, axis=2)-- good = ~np.isnan(errors_full[:, :, 0])-- error_dict = dict()-- for i in range(n_cams):- for j in range(i + 1, n_cams):- subset = good[i] & good[j]- err_subset = errors_norm[:, subset][[i, j]]- err_subset_mean = np.mean(err_subset, axis=0)- if np.sum(subset) > min_points:- percents = np.percentile(err_subset_mean, [15, 75])- # percents = np.percentile(err_subset, [25, 75])- error_dict[(i, j)] = (err_subset.shape[1], percents)- return error_dict---def check_errors(cgroup, imgp):- p3ds = cgroup.triangulate(imgp)- errors_full = cgroup.calculate_reprojection_error(p3ds, imgp, mean=False)- return get_error_dict(errors_full)---def subset_extra(extra, ixs):- if extra is None:- return None-- new_extra = {- "objp": extra["objp"][ixs],- "ids": extra["ids"][ixs],- "rvecs": extra["rvecs"][:, ixs],- "tvecs": extra["tvecs"][:, ixs],- }- return new_extra---def resample_points_extra(imgp, extra, n_samp=25):- n_cams, n_points, _ = imgp.shape- ids = remap_ids(extra["ids"])- n_ids = np.max(ids) + 1- good = ~np.isnan(imgp[:, :, 0])- ixs = np.arange(n_points)-- cam_counts = np.zeros((n_ids, n_cams), dtype="int64")- for idnum in range(n_ids):- cam_counts[idnum] = np.sum(good[:, ids == idnum], axis=1)- cam_counts_random = cam_counts + np.random.random(size=cam_counts.shape)- best_boards = np.argsort(-cam_counts_random, axis=0)-- cam_totals = np.zeros(n_cams, dtype="int64")-- include = set()- for cam_num in range(n_cams):- for board_id in best_boards[:, cam_num]:- include.update(ixs[ids == board_id])- cam_totals += cam_counts[board_id]- if cam_totals[cam_num] >= n_samp or cam_counts_random[board_id, cam_num] < 1:- break-- final_ixs = sorted(include)- newp = imgp[:, final_ixs]- extra = subset_extra(extra, final_ixs)- return newp, extra---def resample_points_based_on_shared_views(points2d: np.ndarray,- extra_data: dict[str, Any] |None = None,- number_of_samples_to_return: int = 25) -> tuple[np.ndarray, dict[str, Any] | None]:- """- 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', 'rvecs', 'tvecs'})- 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.- """-- 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])- num_cams = np.sum(points2d_valid, axis=0) # Number of cameras observing each point-- included_points = set()-- # Iterate over each unique pair of cameras- for first_cam in range(n_cams):- for second_cam in range(first_cam + 1, n_cams):- # Determine which points are visible in both selected cameras- visible_in_both = points2d_valid[first_cam] & points2d_valid[second_cam]- n_good = np.sum(visible_in_both)-- if n_good > 0:- # Select points seen by the most cameras, adding a small random value for tie-breaking- visibility_scores = num_cams[visible_in_both].astype(np.float64)- visibility_scores += np.random.random(size=visibility_scores.shape)-- # Pick the top n_samp points based on visibility scores- picked_indices = np.argsort(-visibility_scores)[:number_of_samples_to_return]- selected_points = point_indices[visible_in_both][picked_indices]-- # Add selected points to the set of included points- included_points.update(selected_points)-- # Sort the final indices of included points for consistent ordering- final_indices = sorted(included_points)- resampled_ponts = points2d[:, final_indices]-- # Subset the extra data if provided- if extra_data is not None:- extra_data = subset_extra_data(extra_data, final_indices)- return resampled_ponts, extra_data-def subset_extra_data(extra_data: dict[str, Any], indices: np.ndarray) -> dict[str, Any]:- """- Subset the extra data dictionary based on the selected indices.-- :param extra_data: The dictionary containing extra data.- :param indices: The indices to subset the data.- :return: A new dictionary with data subset according to the provided indices.- """- if not extra_data:- return extra_data- return {key: value[indices] for key, value in extra_data.items() if key in extra_data}---def medfilt_data(values, size=15):- padsize = size + 5- vpad = np.pad(values, (padsize, padsize), mode="reflect")- vpadf = signal.medfilt(vpad, kernel_size=size)- return vpadf[padsize:-padsize]-def nan_helper(y):- return np.isnan(y), lambda z: z.nonzero()[0]---def interpolate_data(vals):- nans, ix = nan_helper(vals)- out = np.copy(vals)- try:- out[nans] = np.interp(ix(nans), ix(~nans), vals[~nans])- except ValueError:- out[:] = 0- return out--def remap_ids(ids):- unique_ids = np.unique(ids)- ids_out = np.copy(ids)- for i, num in enumerate(unique_ids):- ids_out[ids == num] = i- return ids_out-def transform_points(points, rvecs, tvecs):- """Rotate points by given rotation vectors and translate.- Rodrigues' rotation formula is used.- """- theta = np.linalg.norm(rvecs, axis=1)[:, np.newaxis]- with np.errstate(invalid="ignore"):- v = rvecs / theta- v = np.nan_to_num(v)- dot = np.sum(points * v, axis=1)[:, np.newaxis]- cos_theta = np.cos(theta)- sin_theta = np.sin(theta)- rotated = cos_theta * points + sin_theta * np.cross(v, points) + dot * (1 - cos_theta) * v-- return rotated + tvecsdef get_connections(xs, cam_names=None, both=True):@@ -378,22 +198,3 @@ def get_initial_extrinsics(rtvecs, cam_names=None):rvecs = np.array(rvecs)tvecs = np.array(tvecs)return rvecs, tvecs---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.-- Args:- error_dict (Dict[Tuple[int, int], Tuple[int, np.ndarray]]): Dictionary containing error statistics- for camera pairs, where the key is a tuple of camera indices and the value is a tuple of- (number of observations, percentiles).-- Returns:- Tuple[float, float]: A tuple of (max_error, min_error) based on the percentiles of error means.- """- max_error = 0.0- min_error = float('inf')- for _, (_, percents) in error_dict.items():- max_error = max(percents[-1], max_error)- min_error = min(percents[0], min_error)- return max_error, min_errordiff --git a/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/anipose_stuff/anipose_triangulate_simple.py b/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/anipose_stuff/anipose_triangulate_simple.pyindex 85e345a5..94cba888 100644--- a/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/anipose_stuff/anipose_triangulate_simple.py+++ b/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/anipose_stuff/anipose_triangulate_simple.py@@ -2,29 +2,3 @@ import numpy as npfrom numba import jit-@jit(nopython=True, parallel=False)-def anipose_triangulate_simple(points2d: np.ndarray, camera_matrices: np.ndarray) -> np.ndarray:- """- Triangulate 3D point from 2D points and camera matrices using the Direct Linear Transform (DLT) method.-- :param points2d: A 2D array of shape (N, 2), where N is the number of cameras. Each row represents the- 2D coordinates (x, y) of the point in the image plane of each camera.- :param camera_matrices: A 3D array of shape (N, 3, 4), where each element is a 3x4 camera projection matrix.- :return: A 1D array of shape (3,) representing the triangulated 3D point.- """-- num_cams = camera_matrices.shape[0]- A = np.zeros((num_cams * 2, 4))-- for i in range(num_cams):- x, y = points2d[i]- mat = camera_matrices[i]- 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-- points3d_xyzh = vh[-1]- points3d_xyzh /= points3d_xyzh[3] # Normalize the last element to 1-- return points3d_xyzh[:3]diff --git a/freemocap/pipelines/calibration_pipeline/calibration_aggregation_node.py b/freemocap/pipelines/calibration_pipeline/calibration_aggregation_node.pyindex ccc18596..59cab2f2 100644--- a/freemocap/pipelines/calibration_pipeline/calibration_aggregation_node.py+++ b/freemocap/pipelines/calibration_pipeline/calibration_aggregation_node.py@@ -48,6 +48,7 @@ class CalibrationAggregationNodeConfig(BasePipelineStageConfig):class CameraViewRecord(BaseModel):camera_id: CameraIdcan_see_target: bool+ camera_node_output: CalibrationCameraNodeOutputDataclass SharedViewAccumulator(BaseModel):@@ -86,7 +87,8 @@ class SharedViewAccumulator(BaseModel):def receive_camera_node_output(self, multi_frame_number: int,camera_node_output: dict[CameraId, CalibrationCameraNodeOutputData]):self.camera_view_records_by_frame[multi_frame_number] = {camera_id: CameraViewRecord(camera_id=camera_id,- can_see_target=camera_node_output.can_see_target)+ can_see_target=camera_node_output.can_see_target,+ camera_node_output=camera_node_output)for camera_id, camera_node_output incamera_node_output.items()}diff --git a/freemocap/pipelines/calibration_pipeline/calibration_camera_node.py b/freemocap/pipelines/calibration_pipeline/calibration_camera_node.pyindex 61d12ca7..a50b0f78 100644--- a/freemocap/pipelines/calibration_pipeline/calibration_camera_node.py+++ b/freemocap/pipelines/calibration_pipeline/calibration_camera_node.py@@ -13,7 +13,7 @@ from skellytracker.trackers.charuco_tracker import CharucoTrackerConfig, Charucofrom skellytracker.trackers.charuco_tracker.charuco_observation import CharucoObservationfrom freemocap.pipelines.calibration_pipeline.single_camera_calibrator import SingleCameraCalibrator, \- SingleCameraCalibrationDTO+ SingleCameraCalibrationEstimatefrom freemocap.pipelines.pipeline_abcs import BaseCameraNode, BasePipelineStageConfig, BasePipelineData, \BaseCameraNodeOutputData@@ -31,7 +31,7 @@ class CalibrationPipelineCameraNodeConfig(BasePipelineStageConfig):class CalibrationCameraNodeOutputData(BaseCameraNodeOutputData):frame_metadata: FrameMetadatacharuco_observation: CharucoObservation- calibration_dto: SingleCameraCalibrationDTO+ calibration_estimate: SingleCameraCalibrationEstimate@propertydef can_see_target(self) -> bool:@@ -41,7 +41,7 @@ class CalibrationCameraNodeOutputData(BaseCameraNodeOutputData):return dict(frame_metadata=self.frame_metadata.model_dump(),charuco_observation=self.charuco_observation.to_serializable_dict(),- calibration_dto=self.calibration_dto.to_serializable_dict(),+ calibration_estimate=self.calibration_estimate.to_serializable_dict(),can_see_target=self.can_see_target,)@@ -118,7 +118,7 @@ class CalibrationCameraNode(BaseCameraNode):output_queue.put(CalibrationCameraNodeOutputData(frame_metadata=FrameMetadata.from_frame_metadata_array(frame.metadata),charuco_observation=observation,- calibration_dto=camera_calibration_estimator.to_dto(),+ calibration_estimate=camera_calibration_estimator.to_dto(),time_to_retrieve_frame_ns=time_to_retrieve,time_to_process_frame_ns=time_to_process),diff --git a/freemocap/pipelines/calibration_pipeline/single_camera_calibrator.py b/freemocap/pipelines/calibration_pipeline/single_camera_calibrator.pyindex fcf21f7c..a02fdaaf 100644--- a/freemocap/pipelines/calibration_pipeline/single_camera_calibrator.py+++ b/freemocap/pipelines/calibration_pipeline/single_camera_calibrator.py@@ -18,7 +18,7 @@ DEFAULT_INTRINSICS_COEFFICIENTS_COUNT = 5MIN_CHARUCO_CORNERS = 6@dataclass-class SingleCameraCalibrationDTO:+class SingleCameraCalibrationEstimate:camera_matrix: np.ndarray[3, 3]distortion_coefficients: np.ndarray[..., 1]mean_reprojection_errors: list[float]@@ -241,8 +241,8 @@ class SingleCameraCalibrator:print(f"Mean reprojection errors: {self.mean_reprojection_errors}")- def to_dto(self) -> SingleCameraCalibrationDTO:- return SingleCameraCalibrationDTO(+ def to_dto(self) -> SingleCameraCalibrationEstimate:+ return SingleCameraCalibrationEstimate(camera_matrix=self.camera_matrix,distortion_coefficients=self.distortion_coefficients,mean_reprojection_errors=self.mean_reprojection_errors,