Free Motion Capture for Everyone πβ¨
TypeScript
anipose relatively chunked, i guess
jonmatthis1dd5998408fa
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 0652fdaa..16a56870 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@@ -1,70 +1,46 @@# Most of this was copied (with permission) from the original `aniposelib` package (https://github.com/lambdaloop/aniposelib), and we're adapting it to our needs here. M# ore info on Anipoise: https://anipose.readthedocs.io/en/latest/-import itertoolsimport loggingimport multiprocessing-import time-from collections import defaultdict-from copy import copy+from typing import Any-import cv2import numpy as np-import toml-from aniposelib.boards import extract_points, extract_rtvecs, get_video_params, merge_rows-from aniposelib.utils import get_rtvec, make_M+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_matrixfrom tqdm import trange-from freemocap.core_processes.capture_volume_calibration.anipose_camera_calibration.anipose_stuff.anipose_camera import \- Camera-from freemocap.core_processes.capture_volume_calibration.anipose_camera_calibration.anipose_stuff.anipose_functions import \- get_error_dict, subset_extra, resample_points, medfilt_data, interpolate_data, remap_ids, transform_points, \- get_connections, get_initial_extrinsics-from freemocap.core_processes.capture_volume_calibration.anipose_camera_calibration.anipose_stuff.anipose_triangulate_simple import \- anipose_triangulate_simple-from freemocap.core_processes.capture_volume_calibration.anipose_camera_calibration.anipose_stuff.fisheye_camera import \- FisheyeCamera--logger = logging.getLogger(__name__)class AniposeCameraGroup:- def __init__(self, cameras, metadata={}):+ def __init__(self, cameras: list[AniposeCamera], metadata: dict[str, Any]):self.cameras = camerasself.metadata = metadata- def subset_cameras(self, indices):- cams = [self.cameras[ix].copy() for ix in indices]- return AniposeCameraGroup(cams, self.metadata)-- def subset_cameras_names(self, names):- cur_names = self.get_names()- cur_names_dict = dict(zip(cur_names, range(len(cur_names))))- indices = []- for name in names:- if name not in cur_names_dict:- raise IndexError("name {} not part of camera names: {}".format(name, cur_names))- indices.append(cur_names_dict[name])- return self.subset_cameras(indices)-- def project(self, points3d: np.ndarray):+ 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")+ 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(points3d).reshape(n_points, 2)+ 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}"+ # 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@@ -118,128 +94,30 @@ class AniposeCameraGroup:return triangulated_points3d- def triangulate_possible(- self,- points,- undistort=True,- min_cams=2,- progress=False,- threshold=0.5,- kill_event: multiprocessing.Event = None,- ):- """Given an CxNxPx2 array, this returns an Nx3 array of points- by triangulating all possible points and picking the ones with- best reprojection error- where:- C: number of cameras- N: number of points- P: number of possible options per point- """-- assert points.shape[0] == len(- self.cameras- ), "Invalid points shape, first dim should be equal to" " number of cameras ({}), but shape is {}".format(- len(self.cameras), points.shape- )-- n_cams, n_points, n_possible, _ = points.shape-- cam_nums, point_nums, possible_nums = np.where(~np.isnan(points[:, :, :, 0]))-- all_iters = defaultdict(dict)-- for cam_num, point_num, possible_num in zip(cam_nums, point_nums, possible_nums):- if cam_num not in all_iters[point_num]:- all_iters[point_num][cam_num] = []- all_iters[point_num][cam_num].append((cam_num, possible_num))-- for point_num in all_iters.keys():- for cam_num in all_iters[point_num].keys():- all_iters[point_num][cam_num].append(None)-- out = np.full((n_points, 3), np.nan, dtype="float64")- picked_vals = np.zeros((n_cams, n_points, n_possible), dtype="bool")- errors = np.zeros(n_points, dtype="float64")- points_2d = np.full((n_cams, n_points, 2), np.nan, dtype="float64")-- if progress:- iterator = trange(n_points, ncols=70)- else:- iterator = range(n_points)-- for point_ix in iterator:- best_point = None- best_error = 200-- n_cams_max = len(all_iters[point_ix])-- for picked in itertools.product(*all_iters[point_ix].values()):- picked = [p for p in picked if p is not None]- if len(picked) < min_cams and len(picked) != n_cams_max:- continue-- if kill_event is not None and kill_event.is_set():- return None-- cnums = [p[0] for p in picked]- xnums = [p[1] for p in picked]-- pts = points[cnums, point_ix, xnums]- cc = self.subset_cameras(cnums)-- p3d = cc.triangulate(pts, undistort=undistort)- err = cc.reprojection_error(p3d, pts, mean=True)-- if err < best_error:- best_point = {- "error": err,- "point": p3d[:3],- "points": pts,- "picked": picked,- "joint_ix": point_ix,- }- best_error = err- if best_error < threshold:- break-- if best_point is not None:- out[point_ix] = best_point["point"]- picked = best_point["picked"]- cnums = [p[0] for p in picked]- xnums = [p[1] for p in picked]- picked_vals[cnums, point_ix, xnums] = True- errors[point_ix] = best_point["error"]- points_2d[cnums, point_ix] = best_point["points"]-- # return out, picked_vals, points_2d, errors #original code from OG anipose- return out # simplify output so that `triangulate_ransac` can be used exactly the same way as `triangulate`-- def triangulate_ransac(- self, points, undistort=True, min_cams=2, 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 points.shape[0] == len(- self.cameras- ), "Invalid points shape, first dim should be equal to" " number of cameras ({}), but shape is {}".format(- len(self.cameras), points.shape- )-- n_cams, n_points, _ = points.shape-- points_ransac = points.reshape(n_cams, n_points, 1, 2)-- return self.triangulate_possible(- points_ransac, undistort=undistort, min_cams=min_cams, progress=progress, kill_event=kill_event- )-@jit(parallel=True, forceobj=True)- def reprojection_error(self, points_3d, points_2d, mean=False):+ 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}")++ 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 = Falseif len(points_3d.shape) == 1 and len(points_2d.shape) == 2:@@ -248,10 +126,8 @@ class AniposeCameraGroup:one_point = Truen_cams, n_points, _ = points_2d.shape- assert points_3d.shape == (- n_points,- 3,- ), "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))@@ -274,192 +150,272 @@ class AniposeCameraGroup:return errors- def bundle_adjust_iter(- self,- p2ds,- extra=None,- n_iters=10,- start_mu=15,- end_mu=1,- max_nfev=200,- ftol=1e-4,- n_samp_iter=100,- n_samp_full=1000,- error_threshold=0.3,- verbose=False,- ):- """Given an CxNx2 array of 2D points,- where N is the number of points and C is the number of cameras,- this performs iterative bundle adjustsment to fine-tune the parameters of the cameras.- That is, it performs bundle adjustment multiple times, adjusting the weights given to points- to reduce the influence of outliers.- This is inspired by the algorithm for Fast Global Registration by Zhou, Park, and Koltun+ def 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,+ ) -> 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."""- error_list = []-- assert p2ds.shape[0] == len(- self.cameras- ), "Invalid points shape, first dim should be equal to" " number of cameras ({}), but shape is {}".format(- len(self.cameras), p2ds.shape+ # Validate input shapes+ assert points2d.ndim == 3 and points2d.shape[2] == 2, "points2d must be a CxNx2 array"+ assert points2d.shape[0] == len(self.cameras), (+ f"Invalid points shape, expected first dimension to be equal to "+ f"number of cameras ({len(self.cameras)}), but got shape {points2d.shape}")- p2ds_full = p2ds- extra_full = extra+ # Initialize+ error_list = []+ original_points2d = points2d+ original_extra_data = extra_data- p2ds, extra = resample_points(p2ds_full, extra_full, n_samp=n_samp_full)- error = self.average_error(p2ds, median=True)+ 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}")++ # Resample initial set of points to prioritize shared views+ points2d, extra_data = resample_points_based_on_shared_views(+ original_points2d,+ original_extra_data,+ 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)+ # Calculate dynamic error thresholds for each iteration+ error_thresholds = np.exp(+ np.linspace(np.log(starting_error_threshold), np.log(ending_error_threshold), num=max_iterations))if verbose:- print("error: ", error)+ logger.debug(f"Error thresholds we will use for each bundle adjustment iteration: {error_thresholds}")- mus = np.exp(np.linspace(np.log(start_mu), np.log(end_mu), num=n_iters))+ # Iterative bundle adjustment+ dynamic_error_threshold = starting_error_threshold+ for iteration in range(max_iterations):+ resampled_points2d, resampled_extra_data = resample_points_based_on_shared_views(+ original_points2d, original_extra_data, number_of_samples_to_return=number_of_samples_full+ )- if verbose:- print("n_samples: {}".format(n_samp_iter))+ # Triangulate 3D points from resampled 2D points+ triangulated_points3d = self.triangulate(resampled_points2d)- for i in range(n_iters):- p2ds, extra = resample_points(p2ds_full, extra_full, n_samp=n_samp_full)- p3ds = self.triangulate(p2ds)- errors_full = self.reprojection_error(p3ds, p2ds, mean=False)- errors_norm = self.reprojection_error(p3ds, p2ds, mean=True)+ # Calculate reprojection errors+ errors_full = self.calculate_reprojection_error(+ triangulated_points3d, resampled_points2d, mean=False+ )+ normalized_errors = self.calculate_reprojection_error(+ triangulated_points3d, resampled_points2d, mean=True+ )+ # Compute error statisticserror_dict = get_error_dict(errors_full)- max_error = 0- min_error = 0- for k, v in error_dict.items():- num, percents = v- max_error = max(percents[-1], max_error)- min_error = max(percents[0], min_error)- mu = max(min(max_error, mus[i]), min_error)-- good = errors_norm < mu- extra_good = subset_extra(extra, good)- p2ds_samp, extra_samp = resample_points(p2ds[:, good], extra_good, n_samp=n_samp_iter)-- error = np.median(errors_norm)- error_list.append(error)-- if error < error_threshold:+ max_error, min_error = calculate_error_bounds(error_dict)+ dynamic_error_threshold = max(min(max_error, error_thresholds[iteration]), min_error)++ # 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)++ # Update error list and check for convergence+ 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")breakif verbose:- print(error_dict)- print("error: {:.2f}, mu: {:.1f}, ratio: {:.3f}".format(error, mu, np.mean(good)))- print(f"previous error scores: [magenta] {error_list}[/magenta]")-- self.bundle_adjust(- p2ds_samp,- extra_samp,+ 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 adjustment+ self.bundle_adjust_points2d(+ resampled_points2d[:, good_points_mask],+ filtered_extra_data,loss="linear",- ftol=ftol,- max_nfev=max_nfev,+ function_tolerance=function_tolerance,+ maximum_number_function_evals=max_number_function_evals,verbose=verbose,)- p2ds, extra = resample_points(p2ds_full, extra_full, n_samp=n_samp_full)- p3ds = self.triangulate(p2ds)- errors_full = self.reprojection_error(p3ds, p2ds, mean=False)- errors_norm = self.reprojection_error(p3ds, p2ds, mean=True)- error_dict = get_error_dict(errors_full)- if verbose:- print(error_dict)-- max_error = 0- min_error = 0- for k, v in error_dict.items():- num, percents = v- max_error = max(percents[-1], max_error)- min_error = max(percents[0], min_error)- mu = max(max(max_error, end_mu), min_error)-- good = errors_norm < mu- extra_good = subset_extra(extra, good)- self.bundle_adjust(- p2ds[:, good],- extra_good,- loss="linear",- ftol=ftol,- max_nfev=max(200, max_nfev),- verbose=verbose,+ # 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=number_of_samples_full)+ self.final_adjustments(points2d=final_resampled_points2d,+ 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)++ final_error = self.get_mean_reprojection_error(final_resampled_points2d, median=True)+ if verbose:+ print("Final error: ", final_error)++ return float(final_error)- error = self.average_error(p2ds, median=True)- p3ds = self.triangulate(p2ds)- errors_full = self.reprojection_error(p3ds, p2ds, mean=False)++ def final_adjustments(self,+ points2d: np.ndarray,+ extra_data: dict[str, Any],+ error_threshold: float,+ function_tolerance: float,+ max_number_function_evals: int,+ verbose: bool) -> 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)+ errors_full = self.calculate_reprojection_error(+ triangulated_points3d, points2d, mean=False+ )+ normalized_errors = self.calculate_reprojection_error(+ triangulated_points3d, points2d, mean=True+ )error_dict = get_error_dict(errors_full)if verbose:- print(error_dict)+ logger.debug("Error dictionary after final adjustments", error_dict)- if verbose:- print("error: ", error)-- return error-- def bundle_adjust(- self,- p2ds,- extra=None,- loss="linear",- threshold=50,- ftol=1e-4,- max_nfev=1000,- weights=None,- start_params=None,- verbose=True,- ):- """Given an CxNx2 array of 2D points,- where N is the number of points and C is the number of cameras,- this performs bundle adjustsment to fine-tune the parameters of the cameras"""+ 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)- assert p2ds.shape[0] == len(- self.cameras- ), "Invalid points shape, first dim should be equal to" " number of cameras ({}), but shape is {}".format(- len(self.cameras), p2ds.shape+ self.bundle_adjust_points2d(+ points2d[:, good_points_mask],+ filtered_extra_data,+ loss="linear",+ function_tolerance=function_tolerance,+ maximum_number_function_evals=max(200, max_number_function_evals),+ verbose=verbose,)- if extra is not None:- extra["ids_map"] = remap_ids(extra["ids"])+ def bundle_adjust_points2d(+ self,+ points2d: np.ndarray,+ extra_data: dict[str, Any] = None,+ 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.++ 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.+ """+ # 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.cameras):+ 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}"+ )++ # 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"])- x0, n_cam_params = self._initialize_params_bundle(p2ds, extra)+ # Initialize parameters for bundle adjustment+ initial_params, num_camera_params = self._initalize_bundle_adjust_parameters(points2d, extra_data)+ # Override initial parameters if start_params are providedif start_params is not None:- x0 = start_params- n_cam_params = len(self.cameras[0].get_params())+ initial_params = start_params+ num_camera_params = len(self.cameras[0].get_params())- error_fun = self._error_fun_bundle+ # Define error function and sparsity pattern for optimization- jac_sparse = self._jac_sparsity_bundle(p2ds, n_cam_params, extra)+ jac_sparsity = self._calculate_bundle_adjustment_jacobian_sparsity(points2d, num_camera_params, extra_data)- f_scale = threshold- opt = optimize.least_squares(- error_fun,- x0,- jac_sparsity=jac_sparse,- f_scale=f_scale,+ # Set up optimization options+ optimization_result = optimize.least_squares(+ fun=self._bundle_adjust_error_function,+ x0=initial_params,+ jac_sparsity=jac_sparsity,+ f_scale=threshold,x_scale="jac",loss=loss,- ftol=ftol,+ ftol=function_tolerance,method="trf",tr_solver="lsmr",verbose=2 * verbose,- max_nfev=max_nfev,- args=(p2ds, n_cam_params, extra),+ max_nfev=maximum_number_function_evals,+ args=(points2d, num_camera_params, extra_data),)- best_params = opt.x- for i, cam in enumerate(self.cameras):- a = i * n_cam_params- b = (i + 1) * n_cam_params- cam.set_params(best_params[a:b])+ # Update camera parameters with optimized results+ optimized_params = optimization_result.x+ for i, camera in enumerate(self.cameras):+ start_idx = i * num_camera_params+ end_idx = (i + 1) * num_camera_params+ camera.set_params(optimized_params[start_idx:end_idx])- error = self.average_error(p2ds)- return error+ # Calculate and return the mean reprojection error+ mean_error = self.get_mean_reprojection_error(points2d)+ return float(mean_error)@jit(parallel=True, forceobj=True)- def _error_fun_bundle(self, params, p2ds, n_cam_params, extra):+ def _bundle_adjust_error_function(self, params, points2d, n_cam_params, extra):"""Error function for bundle adjustment"""- good = ~np.isnan(p2ds)+ good = ~np.isnan(points2d)n_cams = len(self.cameras)for i in range(n_cams):@@ -470,9 +426,9 @@ class AniposeCameraGroup:n_cams = len(self.cameras)sub = n_cam_params * n_cams- n3d = p2ds.shape[1] * 3- p3ds_test = params[sub : sub + n3d].reshape(-1, 3)- errors = self.reprojection_error(p3ds_test, p2ds)+ n3d = points2d.shape[1] * 3+ p3ds_test = params[sub: sub + n3d].reshape(-1, 3)+ errors = self.calculate_reprojection_error(p3ds_test, points2d)errors_reproj = errors[good]if extra is not None:@@ -481,8 +437,8 @@ class AniposeCameraGroup:min_scale = np.min(objp[objp > 0])n_boards = int(np.max(ids)) + 1a = 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)+ 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_scaleelse:@@ -490,21 +446,24 @@ class AniposeCameraGroup:return np.hstack([errors_reproj, errors_obj])- def _jac_sparsity_bundle(self, p2ds, n_cam_params, extra):+ def _calculate_bundle_adjustment_jacobian_sparsity(self,+ points2d: np.ndarray,+ number_of_camear_parameters: int,+ extra: dict[str, Any] | None = None) -> 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"""- point_indices = np.zeros(p2ds.shape, dtype="int32")- cam_indices = np.zeros(p2ds.shape, dtype="int32")+ point_indices = np.zeros(points2d.shape, dtype="int32")+ cam_indices = np.zeros(points2d.shape, dtype="int32")- for i in range(p2ds.shape[1]):+ for i in range(points2d.shape[1]):point_indices[:, i] = i- for j in range(p2ds.shape[0]):+ for j in range(points2d.shape[0]):cam_indices[j] = j- good = ~np.isnan(p2ds)+ good = ~np.isnan(points2d)if extra is not None:ids = extra["ids_map"]@@ -514,9 +473,9 @@ class AniposeCameraGroup:n_boards = 0total_board_params = 0- n_cams = p2ds.shape[0]- n_points = p2ds.shape[1]- total_params_reproj = n_cams * n_cam_params + n_points * 3+ n_cams = points2d.shape[0]+ n_points = points2d.shape[1]+ total_params_reproj = n_cams * number_of_camear_parameters + n_points * 3n_params = total_params_reproj + total_board_paramsn_good_values = np.sum(good)@@ -534,12 +493,12 @@ class AniposeCameraGroup:ix = np.arange(n_good_values)## update camera params based on point error- for i in range(n_cam_params):- A_sparse[ix, cam_indices_good * n_cam_params + i] = 1+ for i in range(number_of_camear_parameters):+ A_sparse[ix, cam_indices_good * number_of_camear_parameters + i] = 1## update point position based on point errorfor i in range(3):- A_sparse[ix, n_cams * n_cam_params + point_indices_good * 3 + i] = 1+ 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:@@ -565,12 +524,12 @@ class AniposeCameraGroup:for i in range(3):A_sparse[n_good_values + point_ix * 3 + i,- n_cams * n_cam_params + point_ix * 3 + i,+ n_cams * number_of_camear_parameters + point_ix * 3 + i,] = 1return A_sparse- def _initialize_params_bundle(self, p2ds, extra):+ def _initalize_bundle_adjust_parameters(self, points2d:np.ndarray, extra_data:dict[str, Any] | None):"""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"""@@ -580,15 +539,15 @@ class AniposeCameraGroup:total_cam_params = len(cam_params)- n_cams, n_points, _ = p2ds.shape+ n_cams, n_points, _ = points2d.shapeassert n_cams == len(self.cameras), ("number of cameras in CameraGroup does not " "match number of cameras in 2D points given")- p3ds = self.triangulate(p2ds)+ p3ds = self.triangulate(points2d)- if extra is not None:- ids = extra["ids_map"]+ if extra_data is not None:+ ids = extra_data["ids_map"]n_boards = int(np.max(ids[~np.isnan(ids)])) + 1total_board_params = n_boards * (3 + 3) # rvecs + tvecs@@ -596,12 +555,12 @@ class AniposeCameraGroup:rvecs = np.zeros((n_boards, 3), dtype="float64")tvecs = np.zeros((n_boards, 3), dtype="float64")- if "rvecs" in extra and "tvecs" in extra:- rvecs_all = extra["rvecs"]- tvecs_all = extra["tvecs"]+ 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(p2ds[:, point_id, 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])@@ -615,720 +574,20 @@ class AniposeCameraGroup:x0 = np.zeros(total_cam_params + p3ds.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 + p3ds.size] = p3ds.ravel()- if extra is not None:+ 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] = rvecs.ravel()+ x0[start_board + n_boards * 3: start_board + n_boards * 6] = tvecs.ravel()return x0, n_cam_params- def optim_points(- self,- points,- p3ds,- constraints=[],- constraints_weak=[],- scale_smooth=4,- scale_length=2,- scale_length_weak=0.5,- reproj_error_threshold=15,- reproj_loss="soft_l1",- n_deriv_smooth=1,- scores=None,- verbose=False,- ):- """- Take in an array of 2D points of shape CxNxJx2,- an array of 3D points of shape NxJx3,- and an array of constraints of shape Kx2, where- C: number of camera- N: number of frames- J: number of joints- K: number of constraints- This function creates an optimized array of 3D points of shape NxJx3.-- Example constraints:- constraints = [[0, 1], [1, 2], [2, 3]]- (meaning that lengths of segments 0->1, 1->2, 2->3 are all constant)-- """- assert points.shape[0] == len(- self.cameras- ), "Invalid points shape, first dim should be equal to" " number of cameras ({}), but shape is {}".format(- len(self.cameras), points.shape- )-- n_cams, n_frames, n_joints, _ = points.shape- constraints = np.array(constraints)- constraints_weak = np.array(constraints_weak)-- p3ds_intp = np.apply_along_axis(interpolate_data, 0, p3ds)-- p3ds_med = np.apply_along_axis(medfilt_data, 0, p3ds_intp, size=7)-- default_smooth = 1.0 / np.mean(np.abs(np.diff(p3ds_med, axis=0)))- scale_smooth_full = scale_smooth * default_smooth-- t1 = time.time()-- x0 = self._initialize_params_triangulation(p3ds_intp, constraints, constraints_weak)-- x0[~np.isfinite(x0)] = 0-- jac = self._jac_sparsity_triangulation(points, constraints, constraints_weak, n_deriv_smooth)-- opt2 = optimize.least_squares(- self._error_fun_triangulation,- x0=x0,- jac_sparsity=jac,- loss="linear",- ftol=1e-3,- verbose=2 * verbose,- args=(- points,- constraints,- constraints_weak,- scores,- scale_smooth_full,- scale_length,- scale_length_weak,- reproj_error_threshold,- reproj_loss,- n_deriv_smooth,- ),- )-- p3ds_new2 = opt2.x[: p3ds.size].reshape(p3ds.shape)-- t2 = time.time()-- if verbose:- print("optimization took {:.2f} seconds".format(t2 - t1))-- return p3ds_new2-- def optim_points_possible(- self,- points,- p3ds,- constraints=[],- constraints_weak=[],- scale_smooth=4,- scale_length=2,- scale_length_weak=0.5,- reproj_error_threshold=15,- reproj_loss="soft_l1",- n_deriv_smooth=1,- scores=None,- verbose=False,- ):- """- Take in an array of 2D points of shape CxNxJxPx2,- an array of 3D points of shape NxJx3,- and an array of constraints of shape Kx2, where- C: number of camera- N: number of frames- J: number of joints- P: number of possible options per point- K: number of constraints-- This function creates an optimized array of 3D points of shape NxJx3.-- Example constraints:- constraints = [[0, 1], [1, 2], [2, 3]]- (meaning that lengths of segments 0->1, 1->2, 2->3 are all constant)-- """- assert points.shape[0] == len(- self.cameras- ), "Invalid points shape, first dim should be equal to" " number of cameras ({}), but shape is {}".format(- len(self.cameras), points.shape- )-- n_cams, n_frames, n_joints, n_possible, _ = points.shape- constraints = np.array(constraints)- constraints_weak = np.array(constraints_weak)-- p3ds_intp = np.apply_along_axis(interpolate_data, 0, p3ds)-- p3ds_med = np.apply_along_axis(medfilt_data, 0, p3ds_intp, size=7)-- default_smooth = 1.0 / np.mean(np.abs(np.diff(p3ds_med, axis=0)))- scale_smooth_full = scale_smooth * default_smooth-- t1 = time.time()-- x0 = self._initialize_params_triangulation_possible(- p3ds_intp,- points,- constraints=constraints,- constraints_weak=constraints_weak,- )-- print("getting jacobian...")- jac = self._jac_sparsity_triangulation_possible(- points,- constraints=constraints,- constraints_weak=constraints_weak,- n_deriv_smooth=n_deriv_smooth,- )-- beta = 5-- print("starting optimization...")- opt2 = optimize.least_squares(- self._error_fun_triangulation_possible,- x0=x0,- jac_sparsity=jac,- loss="linear",- ftol=1e-3,- verbose=2 * verbose,- args=(- points,- beta,- constraints,- constraints_weak,- scores,- scale_smooth_full,- scale_length,- scale_length_weak,- reproj_error_threshold,- reproj_loss,- n_deriv_smooth,- ),- )- params = opt2.x-- p3ds_new2 = params[: p3ds.size].reshape(p3ds.shape)-- bad = np.isnan(points[:, :, :, :, 0])- all_bad = np.all(bad, axis=3)-- n_params_norm = p3ds.size + len(constraints) + len(constraints_weak)-- alphas = np.zeros((n_cams, n_frames, n_joints, n_possible), dtype="float64")- alphas[~bad] = params[n_params_norm:]-- alphas_exp = np.exp(beta * alphas)- alphas_exp[bad] = 0- alphas_sum = np.sum(alphas_exp, axis=3)- alphas_sum[all_bad] = 1- alphas_norm = alphas_exp / alphas_sum[:, :, :, None]- alphas_norm[bad] = np.nan-- t2 = time.time()-- if verbose:- print("optimization took {:.2f} seconds".format(t2 - t1))-- return p3ds_new2, alphas_norm-- def triangulate_optim(self, points, init_ransac=False, init_progress=False, **kwargs):- """- Take in an array of 2D points of shape CxNxJx2, and an array of constraints of shape Kx2, where- C: number of camera- N: number of frames- J: number of joints- K: number of constraints-- This function creates an optimized array of 3D points of shape NxJx3.-- Example constraints:- constraints = [[0, 1], [1, 2], [2, 3]]- (meaning that lengths of segments 0->1, 1->2, 2->3 are all constant)-- """-- assert points.shape[0] == len(- self.cameras- ), "Invalid points shape, first dim should be equal to" " number of cameras ({}), but shape is {}".format(- len(self.cameras), points.shape- )-- n_cams, n_frames, n_joints, _ = points.shape- # constraints = np.array(constraints)- # constraints_weak = np.array(constraints_weak)-- points_shaped = points.reshape(n_cams, n_frames * n_joints, 2)- if init_ransac:- p3ds, picked, p2ds, errors = self.triangulate_ransac(points_shaped, progress=init_progress)- points = p2ds.reshape(points.shape)- else:- p3ds = self.triangulate(points_shaped, progress=init_progress)- p3ds = p3ds.reshape((n_frames, n_joints, 3))-- c = np.isfinite(p3ds[:, :, 0])- if np.sum(c) < 20:- print("warning: not enough 3D points to calculate_center_of_mass optimization")- return p3ds-- return self.optim_points(points, p3ds, **kwargs)-- @jit(forceobj=True, parallel=True)- def _error_fun_triangulation(- self,- params,- p2ds,- constraints=[],- constraints_weak=[],- scores=None,- scale_smooth=10000,- scale_length=1,- scale_length_weak=0.2,- reproj_error_threshold=100,- reproj_loss="soft_l1",- n_deriv_smooth=1,- ):- n_cams, n_frames, n_joints, _ = p2ds.shape-- n_3d = n_frames * n_joints * 3- n_constraints = len(constraints)- n_constraints_weak = len(constraints_weak)-- # load params- p3ds = params[:n_3d].reshape((n_frames, n_joints, 3))- joint_lengths = np.array(params[n_3d : n_3d + n_constraints])- joint_lengths_weak = np.array(params[n_3d + n_constraints :])-- # reprojection errors- p3ds_flat = p3ds.reshape(-1, 3)- p2ds_flat = p2ds.reshape((n_cams, -1, 2))- errors = self.reprojection_error(p3ds_flat, p2ds_flat)- if scores is not None:- scores_flat = scores.reshape((n_cams, -1))- errors = errors * scores_flat[:, :, None]- errors_reproj = errors[~np.isnan(p2ds_flat)]-- rp = reproj_error_threshold- errors_reproj = np.abs(errors_reproj)- if reproj_loss == "huber":- bad = errors_reproj > rp- errors_reproj[bad] = rp * (2 * np.sqrt(errors_reproj[bad] / rp) - 1)- elif reproj_loss == "linear":- pass- elif reproj_loss == "soft_l1":- errors_reproj = rp * 2 * (np.sqrt(1 + errors_reproj / rp) - 1)-- # temporal constraint- errors_smooth = np.diff(p3ds, n=n_deriv_smooth, axis=0).ravel() * scale_smooth-- # joint length constraint- errors_lengths = np.empty((n_constraints, n_frames), dtype="float64")- for cix, (a, b) in enumerate(constraints):- lengths = np.linalg.norm(p3ds[:, a] - p3ds[:, b], axis=1)- expected = joint_lengths[cix]- errors_lengths[cix] = 100 * (lengths - expected) / expected- errors_lengths = errors_lengths.ravel() * scale_length-- errors_lengths_weak = np.empty((n_constraints_weak, n_frames), dtype="float64")- for cix, (a, b) in enumerate(constraints_weak):- lengths = np.linalg.norm(p3ds[:, a] - p3ds[:, b], axis=1)- expected = joint_lengths_weak[cix]- errors_lengths_weak[cix] = 100 * (lengths - expected) / expected- errors_lengths_weak = errors_lengths_weak.ravel() * scale_length_weak-- return np.hstack([errors_reproj, errors_smooth, errors_lengths, errors_lengths_weak])-- def _error_fun_triangulation_possible(self, params, p2ds, beta=2, constraints=[], constraints_weak=[], *args):- # extract alphas from end of params- # soft argmax for picking the appropriate points from p2ds- # pass the points to error_fun_triangulate_possible for residuals- # add errors to keep the alphas in check- # return all the errors-- n_cams, n_frames, n_joints, n_possible, _ = p2ds.shape-- n_3d = n_frames * n_joints * 3- n_constraints = len(constraints)- n_constraints_weak = len(constraints_weak)- n_params_norm = n_3d + n_constraints + n_constraints_weak-- # load params- bad = np.isnan(p2ds[:, :, :, :, 0])- all_bad = np.all(bad, axis=3)-- alphas = np.zeros((n_cams, n_frames, n_joints, n_possible), dtype="float64")- alphas[~bad] = params[n_params_norm:]- params_rest = np.array(params[:n_params_norm])-- # get normalized alphas- alphas_exp = np.exp(beta * alphas)- alphas_exp[bad] = 0- alphas_sum = np.sum(alphas_exp, axis=3)- alphas_sum[all_bad] = 1- alphas_norm = alphas_exp / alphas_sum[:, :, :, None]-- # extract the 2D points using soft argmax- p2ds_test = np.copy(p2ds)- p2ds_test[bad] = 0- p2ds_adj = np.sum(alphas_norm[:, :, :, :, None] * p2ds_test, axis=3)- p2ds_adj[all_bad] = np.nan-- errors = self._error_fun_triangulation(params_rest, p2ds_adj, constraints, constraints_weak, *args)-- alphas_test = alphas_norm[~all_bad]- errors_alphas = (1 - np.std(alphas_test, axis=1)) * 10-- return np.hstack([errors, errors_alphas])-- def _initialize_params_triangulation(self, p3ds, constraints=[], constraints_weak=[]):- joint_lengths = np.empty(len(constraints), dtype="float64")- joint_lengths_weak = np.empty(len(constraints_weak), dtype="float64")-- for cix, (a, b) in enumerate(constraints):- lengths = np.linalg.norm(p3ds[:, a] - p3ds[:, b], axis=1)- joint_lengths[cix] = np.median(lengths)-- for cix, (a, b) in enumerate(constraints_weak):- lengths = np.linalg.norm(p3ds[:, a] - p3ds[:, b], axis=1)- joint_lengths_weak[cix] = np.median(lengths)-- all_lengths = np.hstack([joint_lengths, joint_lengths_weak])- med = np.median(all_lengths)- if med == 0:- med = 1e-3-- mad = np.median(np.abs(all_lengths - med))-- joint_lengths[joint_lengths == 0] = med- joint_lengths_weak[joint_lengths_weak == 0] = med- joint_lengths[joint_lengths > med + mad * 5] = med- joint_lengths_weak[joint_lengths_weak > med + mad * 5] = med-- return np.hstack([p3ds.ravel(), joint_lengths, joint_lengths_weak])-- def _initialize_params_triangulation_possible(self, p3ds, p2ds, **kwargs):- # initialize params using above function- # initialize alphas to 1 for first one and 0 for other possible-- n_cams, n_frames, n_joints, n_possible, _ = p2ds.shape- good = ~np.isnan(p2ds[:, :, :, :, 0])-- alphas = np.zeros((n_cams, n_frames, n_joints, n_possible), dtype="float64")- alphas[:, :, :, 0] = 0-- params = self._initialize_params_triangulation(p3ds, **kwargs)- params_full = np.hstack([params, alphas[good]])-- return params_full-- def _jac_sparsity_triangulation(self, p2ds, constraints=[], constraints_weak=[], n_deriv_smooth=1):- n_cams, n_frames, n_joints, _ = p2ds.shape- n_constraints = len(constraints)- n_constraints_weak = len(constraints_weak)-- p2ds_flat = p2ds.reshape((n_cams, -1, 2))-- point_indices = np.zeros(p2ds_flat.shape, dtype="int32")- for i in range(p2ds_flat.shape[1]):- point_indices[:, i] = i-- point_indices_3d = np.arange(n_frames * n_joints).reshape((n_frames, n_joints))-- good = ~np.isnan(p2ds_flat)- n_errors_reproj = np.sum(good)- n_errors_smooth = (n_frames - n_deriv_smooth) * n_joints * 3- n_errors_lengths = n_constraints * n_frames- n_errors_lengths_weak = n_constraints_weak * n_frames-- n_errors = n_errors_reproj + n_errors_smooth + n_errors_lengths + n_errors_lengths_weak-- n_3d = n_frames * n_joints * 3- n_params = n_3d + n_constraints + n_constraints_weak-- point_indices_good = point_indices[good]-- A_sparse = dok_matrix((n_errors, n_params), dtype="int16")-- # constraints for reprojection errors- ix_reproj = np.arange(n_errors_reproj)- for k in range(3):- A_sparse[ix_reproj, point_indices_good * 3 + k] = 1-- # sparse constraints for smoothness in time- frames = np.arange(n_frames - n_deriv_smooth)- for j in range(n_joints):- for n in range(n_deriv_smooth + 1):- pa = point_indices_3d[frames, j]- pb = point_indices_3d[frames + n, j]- for k in range(3):- A_sparse[n_errors_reproj + pa * 3 + k, pb * 3 + k] = 1-- ## -- strong constraints --- # joint lengths should change with joint lengths errors- start = n_errors_reproj + n_errors_smooth- frames = np.arange(n_frames)- for cix, (a, b) in enumerate(constraints):- A_sparse[start + cix * n_frames + frames, n_3d + cix] = 1-- # points should change accordingly to match joint lengths too- frames = np.arange(n_frames)- for cix, (a, b) in enumerate(constraints):- pa = point_indices_3d[frames, a]- pb = point_indices_3d[frames, b]- for k in range(3):- A_sparse[start + cix * n_frames + frames, pa * 3 + k] = 1- A_sparse[start + cix * n_frames + frames, pb * 3 + k] = 1-- ## -- weak constraints --- # joint lengths should change with joint lengths errors- start = n_errors_reproj + n_errors_smooth + n_errors_lengths- frames = np.arange(n_frames)- for cix, (a, b) in enumerate(constraints_weak):- A_sparse[start + cix * n_frames + frames, n_3d + n_constraints + cix] = 1-- # points should change accordingly to match joint lengths too- frames = np.arange(n_frames)- for cix, (a, b) in enumerate(constraints_weak):- pa = point_indices_3d[frames, a]- pb = point_indices_3d[frames, b]- for k in range(3):- A_sparse[start + cix * n_frames + frames, pa * 3 + k] = 1- A_sparse[start + cix * n_frames + frames, pb * 3 + k] = 1-- return A_sparse-- def _jac_sparsity_triangulation_possible(self, p2ds_full, **kwargs):- # initialize sparse jacobian using above function- # extend to include alphas from parameters- ## TODO: this initialization is really slow for some reason-- n_cams, n_frames, n_joints, n_possible, _ = p2ds_full.shape- good_full = ~np.isnan(p2ds_full[:, :, :, :, 0])- any_good = np.any(good_full, axis=3)-- n_alphas = np.sum(good_full)- n_errors_alphas = np.sum(any_good)-- p2ds = p2ds_full[:, :, :, 0]- A_sparse = self._jac_sparsity_triangulation(p2ds, **kwargs)-- n_errors, n_params = A_sparse.shape-- B_sparse = dok_matrix((n_errors + n_errors_alphas, n_params + n_alphas), dtype="int16")- for r, c in zip(*A_sparse.nonzero()):- B_sparse[r, c] = A_sparse[r, c]-- point_indices_2d = np.arange(n_cams * n_frames * n_joints).reshape(n_cams, n_frames, n_joints)- point_indices_2d_rep = np.repeat(point_indices_2d[:, :, :, None], 2, axis=3)- point_indices_2d_good = point_indices_2d_rep[~np.isnan(p2ds)]- point_indices_good = point_indices_2d[any_good]-- alpha_indices = np.zeros((n_cams, n_frames, n_joints, n_possible), dtype="int64")- for pnum in range(n_possible):- alpha_indices[:, :, :, pnum] = point_indices_2d-- alpha_indices_good = alpha_indices[good_full]-- # alphas should change according to the reprojection error for each corresponding point- point_indices_2d_good_find = defaultdict(list)- for ix, p in enumerate(point_indices_2d_good):- point_indices_2d_good_find[p].append(ix)-- for ix, alpha_index in enumerate(alpha_indices_good):- B_sparse[point_indices_2d_good_find[alpha_index], n_params + ix] = 1-- # alphas should change according to the alpha errors- point_indices_good_find = dict()- for ix, p in enumerate(point_indices_good):- point_indices_good_find[p] = ix-- for ix, alpha_index in enumerate(alpha_indices_good):- if alpha_index in point_indices_good_find:- err_ix = n_errors + point_indices_good_find[alpha_index]- B_sparse[err_ix, n_params + ix] = 1-- return B_sparse-- def copy(self):- cameras = [cam.copy() for cam in self.cameras]- metadata = copy(self.metadata)- return AniposeCameraGroup(cameras, metadata)-- def set_rotations(self, rvecs):- for cam, rvec in zip(self.cameras, rvecs):- cam.set_rotation(rvec)-- def set_translations(self, tvecs):- for cam, tvec in zip(self.cameras, tvecs):- cam.set_translation(tvec)-- def get_rotations(self):- rvecs = []- for cam in self.cameras:- rvec = cam.get_rotation()- rvecs.append(rvec)- return np.array(rvecs)-- def get_translations(self):- tvecs = []- for cam in self.cameras:- tvec = cam.get_translation()- tvecs.append(tvec)- return np.array(tvecs)-- def get_names(self):- return [cam.get_name() for cam in self.cameras]-- def set_names(self, names):- for cam, name in zip(self.cameras, names):- cam.set_name(name)-- def average_error(self, p2ds, median=False):- p3ds = self.triangulate(p2ds)- errors = self.reprojection_error(p3ds, p2ds, mean=True)+ def get_mean_reprojection_error(self, points2d, median=False):+ triangulated_points3d = self.triangulate(points2d)+ errors = self.calculate_reprojection_error(triangulated_points3d, points2d, mean=True)if median:return np.median(errors)else:return np.mean(errors)-- def calibrate_rows(- self,- all_rows,- board,- init_intrinsics=True,- init_extrinsics=True,- verbose=True,- **kwargs,- ):- assert len(all_rows) == len(self.cameras), "Number of camera detections does not match number of cameras"-- for rows, camera in zip(all_rows, self.cameras):- size = camera.get_size()-- assert size is not None, "Camera with name {} has no specified frame size".format(camera.get_name())-- if init_intrinsics:- objp, imgp = board.get_all_calibration_points(rows)- mixed = [(o, i) for (o, i) in zip(objp, imgp) if len(o) >= 7]- assert len(objp) != 0 and len(imgp) != 0, "No Charuco board points detected"- objp, imgp = zip(*mixed)- matrix = cv2.initCameraMatrix2D(objp, imgp, tuple(size))- camera.set_camera_matrix(matrix)-- for i, (row, cam) in enumerate(zip(all_rows, self.cameras)):- all_rows[i] = board.estimate_pose_rows(cam, row)-- charuco_frames = [f["framenum"][1] for f in all_rows[0]]- merged = merge_rows(all_rows)- imgp, extra = extract_points(merged, board, min_cameras=2)-- if init_extrinsics:- rtvecs = extract_rtvecs(merged)- if verbose:- print(get_connections(rtvecs, self.get_names()))- rvecs, tvecs = get_initial_extrinsics(rtvecs)- self.set_rotations(rvecs)- self.set_translations(tvecs)-- error = self.bundle_adjust_iter(imgp, extra, verbose=verbose, error_threshold=1)-- return error, merged, charuco_frames-- def get_rows_videos(self, videos, board, verbose=True):- all_rows = []-- for cix, (cam, cam_videos) in enumerate(zip(self.cameras, videos)):- rows_cam = []- for vnum, vidname in enumerate(cam_videos):- if verbose:- print(vidname)- rows = board.detect_video(vidname, prefix=vnum, progress=verbose)- if verbose:- print("{} boards detected".format(len(rows)))- rows_cam.extend(rows)- all_rows.append(rows_cam)-- return all_rows-- def set_camera_sizes_videos(self, videos):- for cix, (cam, cam_videos) in enumerate(zip(self.cameras, videos)):- rows_cam = []- for vnum, vidname in enumerate(cam_videos):- params = get_video_params(vidname)- size = (params["width"], params["height"])- cam.set_size(size)-- def calibrate_videos(- self,- videos,- board,- init_intrinsics=True,- init_extrinsics=True,- verbose=True,- **kwargs,- ):- """Takes as input a list of list of video filenames, one list of each camera.- Also takes a board which specifies what should be detected in the videos"""-- all_rows = self.get_rows_videos(videos, board, verbose=verbose)- if init_extrinsics:- self.set_camera_sizes_videos(videos)-- error, merged, charuco_frames = self.calibrate_rows(- all_rows,- board,- init_intrinsics=init_intrinsics,- init_extrinsics=init_extrinsics,- verbose=verbose,- **kwargs,- )- return error, merged, charuco_frames-- def get_dicts(self):- out = []- for cam in self.cameras:- out.append(cam.get_dict())- return out-- def from_dicts(arr):- cameras = []- for d in arr:- if "fisheye" in d and d["fisheye"]:- cam = FisheyeCamera.from_dict(d)- else:- cam = Camera.from_dict(d)- cameras.append(cam)- return AniposeCameraGroup(cameras)-- @staticmethod- def from_names(names, fisheye=False):- cameras = []- for name in names:- if fisheye:- cam = FisheyeCamera(name=name)- else:- cam = Camera(name=name)- cameras.append(cam)- return AniposeCameraGroup(cameras)-- def load_dicts(self, arr):- for cam, d in zip(self.cameras, arr):- cam.load_dict(d)-- def dump(self, fname):- dicts = self.get_dicts()- names = ["cam_{}".format(i) for i in range(len(dicts))]- master_dict = dict(zip(names, dicts))- master_dict["metadata"] = self.metadata- with open(fname, "w") as f:- toml.dump(master_dict, f)-- @staticmethod- def load(fname):- master_dict = toml.load(fname)- keys = sorted(master_dict.keys())- items = [master_dict[k] for k in keys if k != "metadata"]- cgroup = AniposeCameraGroup.from_dicts(items)- if "metadata" in master_dict:- cgroup.metadata = master_dict["metadata"]- return cgroup-- def resize_cameras(self, scale):- for cam in self.cameras:- cam.resize_camera(scale)--diff --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.pyindex 46178ca0..a367770c 100644--- 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.py@@ -3,7 +3,7 @@ import numpy as npfrom aniposelib.utils import make_M-class Camera:+class AniposeCamera:def __init__(self,matrix=np.eye(3),@@ -46,45 +46,45 @@ class Camera:return camdef get_camera_matrix(self):- return self.matrix+ return self.camera_matrixdef get_distortions(self):- return self.dist+ return self.distortion_coefficientsdef set_camera_matrix(self, matrix):- self.matrix = np.array(matrix, dtype="float64")+ self.camera_matrix = np.array(matrix, dtype="float64")def set_focal_length(self, fx, fy=None):if fy is None:fy = fx- self.matrix[0, 0] = fx- self.matrix[1, 1] = fy+ self.camera_matrix[0, 0] = fx+ self.camera_matrix[1, 1] = fydef get_focal_length(self, both=False):- fx = self.matrix[0, 0]- fy = self.matrix[1, 1]+ fx = self.camera_matrix[0, 0]+ fy = self.camera_matrix[1, 1]if both:return (fx, fy)else:return (fx + fy) / 2.0def set_distortions(self, dist):- self.dist = np.array(dist, dtype="float64").ravel()+ self.distortion_coefficients = np.array(dist, dtype="float64").ravel()def set_rotation(self, rvec):- self.rvec = np.array(rvec, dtype="float64").ravel()+ self.rotation_vector = np.array(rvec, dtype="float64").ravel()def get_rotation(self):- return self.rvec+ return self.rotation_vectordef set_translation(self, tvec):- self.tvec = np.array(tvec, dtype="float64").ravel()+ self.translation_vector = np.array(tvec, dtype="float64").ravel()def get_translation(self):- return self.tvec+ return self.translation_vectordef get_extrinsics_mat(self):- return make_M(self.rvec, self.tvec)+ return make_M(self.rotation_vector, self.translation_vector)def get_name(self):return self.name@@ -132,48 +132,31 @@ class Camera:dist[1] = params[8]self.set_distortions(dist)- def distort_points(self, points):- shape = points.shape- points = points.reshape(-1, 1, 2)- new_points = np.dstack([points, np.ones((points.shape[0], 1, 1))])- out, _ = cv2.projectPoints(- new_points,- np.zeros(3),- np.zeros(3),- self.matrix.astype("float64"),- self.dist.astype("float64"),- )- return out.reshape(shape)def undistort_points(self, points):shape = points.shapepoints = points.reshape(-1, 1, 2)- out = cv2.undistortPoints(points, self.matrix.astype("float64"), self.dist.astype("float64"))+ out = cv2.undistortPoints(points, self.camera_matrix.astype("float64"), self.distortion_coefficients.astype("float64"))return out.reshape(shape)- def project(self, points):- points = points.reshape(-1, 1, 3)- out, _ = cv2.projectPoints(- points,- self.rvec,- self.tvec,- self.matrix.astype("float64"),- self.dist.astype("float64"),+ 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"),)- return out+ 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_pointsdef 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- def copy(self):- return Camera(- matrix=self.get_camera_matrix().copy(),- dist=self.get_distortions().copy(),- size=self.get_size(),- rvec=self.get_rotation().copy(),- tvec=self.get_translation().copy(),- name=self.get_name(),- extra_dist=self.extra_dist,- )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 31977a3b..1909a644 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@@ -1,14 +1,26 @@import queuefrom collections import defaultdict, Counter+from typing import Anyimport cv2import numpy as np-from aniposelib.utils import make_M, get_rtvecfrom scipy import signalfrom 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, tvecdef get_error_dict(errors_full, min_points=10):n_cams = errors_full.shape[0]@@ -32,7 +44,7 @@ def get_error_dict(errors_full, min_points=10):def check_errors(cgroup, imgp):p3ds = cgroup.triangulate(imgp)- errors_full = cgroup.reprojection_error(p3ds, imgp, mean=False)+ errors_full = cgroup.calculate_reprojection_error(p3ds, imgp, mean=False)return get_error_dict(errors_full)@@ -78,34 +90,68 @@ def resample_points_extra(imgp, extra, n_samp=25):return newp, extra-def resample_points(imgp, extra=None, n_samp=25):- # if extra is not None:- # return resample_points_extra(imgp, extra, n_samp)+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 = imgp.shape[0]- good = ~np.isnan(imgp[:, :, 0])- ixs = np.arange(imgp.shape[1])+ 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- num_cams = np.sum(~np.isnan(imgp[:, :, 0]), axis=0)+ included_points = set()- include = 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)- for i in range(n_cams):- for j in range(i + 1, n_cams):- subset = good[i] & good[j]- n_good = np.sum(subset)if n_good > 0:- ## pick points, prioritizing points seen by more cameras- arr = np.copy(num_cams[subset]).astype("float64")- arr += np.random.random(size=arr.shape)- picked_ix = np.argsort(-arr)[:n_samp]- picked = ixs[subset][picked_ix]- include.update(picked)+ # 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)- final_ixs = sorted(include)- newp = imgp[:, final_ixs]- extra = subset_extra(extra, final_ixs)- return newp, extra+ # 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):@@ -332,3 +378,22 @@ 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/triangulate_3d_data.py b/freemocap/core_processes/capture_volume_calibration/triangulate_3d_data.pyindex b4907e2c..245848d8 100644--- a/freemocap/core_processes/capture_volume_calibration/triangulate_3d_data.py+++ b/freemocap/core_processes/capture_volume_calibration/triangulate_3d_data.py@@ -50,8 +50,8 @@ def triangulate_3d_data(spatial_data3d_numFrames_numTrackedPoints_XYZ = data3d_flat.reshape(number_of_frames, number_of_tracked_points, 3)- data3d_reprojectionError_flat = anipose_calibration_object.reprojection_error(data3d_flat, data2d_flat, mean=True)- data3d_reprojectionError_full = anipose_calibration_object.reprojection_error(data3d_flat, data2d_flat, mean=False)+ data3d_reprojectionError_flat = anipose_calibration_object.calculate_reprojection_error(data3d_flat, data2d_flat, mean=True)+ data3d_reprojectionError_full = anipose_calibration_object.calculate_reprojection_error(data3d_flat, data2d_flat, mean=False)reprojectionError_cam_frame_marker = np.linalg.norm(data3d_reprojectionError_full, axis=2).reshape(number_of_cameras, number_of_frames, number_of_tracked_points)diff --git a/pyproject.toml b/pyproject.tomlindex 2ce3658b..1b6ec99f 100644--- a/pyproject.toml+++ b/pyproject.toml@@ -12,7 +12,7 @@ dependencies = ["opencv-contrib-python>=4.8.1.78","pyinstaller>=6.11.0","requests>=2.32.3",-# "skelly-synchronize",+ # "skelly-synchronize","skelly-viewer","skellycam","skellyforge",