Skip to content

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.py
index 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 itertools
import logging
import multiprocessing
-import time
-from collections import defaultdict
-from copy import copy
+from typing import Any
-import cv2
import 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_simple
from numba import jit
from scipy import optimize
from scipy.linalg import inv as inverse
from scipy.sparse import dok_matrix
from 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 = cameras
self.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 = False
if len(points_3d.shape) == 1 and len(points_2d.shape) == 2:
@@ -248,10 +126,8 @@ class AniposeCameraGroup:
one_point = True
n_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 statistics
error_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")
break
if 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 provided
if 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)) + 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)
+ 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:
@@ -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 = 0
total_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 * 3
n_params = total_params_reproj + total_board_params
n_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 error
for 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,
] = 1
return 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.shape
assert 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)])) + 1
total_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.py
index 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 np
from aniposelib.utils import make_M
-class Camera:
+class AniposeCamera:
def __init__(
self,
matrix=np.eye(3),
@@ -46,45 +46,45 @@ class Camera:
return cam
def get_camera_matrix(self):
- return self.matrix
+ return self.camera_matrix
def get_distortions(self):
- return self.dist
+ return self.distortion_coefficients
def 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] = fy
def 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.0
def 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_vector
def 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_vector
def 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.shape
points = 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_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
- 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.py
index 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 queue
from collections import defaultdict, Counter
+from typing import Any
import cv2
import numpy as np
-from aniposelib.utils import make_M, get_rtvec
from scipy import signal
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]
@@ -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_error
diff --git a/freemocap/core_processes/capture_volume_calibration/triangulate_3d_data.py b/freemocap/core_processes/capture_volume_calibration/triangulate_3d_data.py
index 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.toml
index 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",