Skip to content

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.py
index 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 dataclass
from typing import Any
+import cv2
import 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_simple
from numba import jit
from scipy import optimize
from scipy.linalg import inv as inverse
from scipy.sparse import dok_matrix
+from skellycam import CameraId
from tqdm import trange
logger = 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.3
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,
+ extra_data: dict[str, Any],
+
) -> float:
"""Performs iterative bundle adjustment to refine camera parameters.
@@ -186,9 +360,9 @@ class AniposeCameraGroup:
"""
# 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), (
+ 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 = points2d
original_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 views
points2d, 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 iteration
error_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 adjustment
dynamic_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 adjustment
self.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 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):
+ 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 provided
if 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 results
optimized_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_params
end_idx = (i + 1) * num_camera_params
camera.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_cams
n3d = 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_vectors
n_cams = points2d.shape[0]
n_points = points2d.shape[1]
@@ -479,10 +769,9 @@ class AniposeCameraGroup:
n_params = total_params_reproj + total_board_params
n_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,
+ ] = 1
return 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] = tvec
x0 = 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_params
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.py
deleted file mode 100644
index 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.py
index 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, fcluster
from scipy.cluster.vq import whiten
from 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 + tvecs
def 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_error
diff --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.py
index 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 np
from 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.py
index 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: CameraId
can_see_target: bool
+ camera_node_output: CalibrationCameraNodeOutputData
class 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 in
camera_node_output.items()}
diff --git a/freemocap/pipelines/calibration_pipeline/calibration_camera_node.py b/freemocap/pipelines/calibration_pipeline/calibration_camera_node.py
index 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, Charuco
from skellytracker.trackers.charuco_tracker.charuco_observation import CharucoObservation
from freemocap.pipelines.calibration_pipeline.single_camera_calibrator import SingleCameraCalibrator, \
- SingleCameraCalibrationDTO
+ SingleCameraCalibrationEstimate
from freemocap.pipelines.pipeline_abcs import BaseCameraNode, BasePipelineStageConfig, BasePipelineData, \
BaseCameraNodeOutputData
@@ -31,7 +31,7 @@ class CalibrationPipelineCameraNodeConfig(BasePipelineStageConfig):
class CalibrationCameraNodeOutputData(BaseCameraNodeOutputData):
frame_metadata: FrameMetadata
charuco_observation: CharucoObservation
- calibration_dto: SingleCameraCalibrationDTO
+ calibration_estimate: SingleCameraCalibrationEstimate
@property
def 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.py
index 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 = 5
MIN_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,