Skip to content

Free Motion Capture for Everyone πŸ’€βœ¨

TypeScript

Ajc/triangulate with outlier rejection (#758)

ajc274e87df3a4d04
diff --git a/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/freemocap_anipose.py b/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/freemocap_anipose.py
index be31ee65..50986fb5 100644
--- a/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/freemocap_anipose.py
+++ b/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/freemocap_anipose.py
@@ -67,6 +67,128 @@ def triangulate_simple(points, camera_mats):
return p3d
+def project_point_to_cam(p3d, P):
+ """
+ Project 3D world point to 2D image coordinates.
+
+ Args:
+ p3d: (3,) array, 3D point [X, Y, Z]
+ P: (3,4) array, camera projection matrix
+
+ Returns:
+ (2,) array, normalized image coordinates [u, v]
+
+ Note: Performs homogeneous projection: [u, v] = (P @ [X, Y, Z, 1])[:2] / (P @ [X, Y, Z, 1])[2]
+ """
+ hom = np.array([p3d[0], p3d[1], p3d[2], 1.0])
+ x = P @ hom
+ return x[:2] / x[2]
+
+
+def triangulate_with_outlier_rejection(
+ subp: np.ndarray,
+ cam_mats: np.ndarray,
+ minimum_cameras_for_triangulation: int = 2,
+ maximum_cameras_to_drop: int = 1,
+ target_reprojection_error: float = 0.01,
+):
+ """
+ Triangulates 3D points with progressive outlier rejection.
+
+ Performs triangulation using all valid cameras, and if reprojection error
+ exceeds threshold, iteratively tests camera combinations to find optimal subset.
+
+ Args:
+ subp: (N,2) array of 2D detections from N valid cameras
+ cam_mats: (N,3,4) array of camera matrices for valid cameras
+ minimum_cameras_for_triangulation: Minimum number of cameras required for triangulation
+ maximum_cameras_to_drop: Maximum number of cameras to drop
+ target_reprojection_error: Maximum acceptable mean reprojection error (normalized coordinates, range ~[-1,1])
+ Returns:
+ tuple[np.ndarray, np.ndarray]: (3D point coordinates, normalized camera weights)
+ """
+
+ # Initialize camera indices and weights
+ total_valid_cams = len(cam_mats)
+ local_indices = list(range(total_valid_cams))
+ normalized_camera_weights = np.zeros(total_valid_cams)
+
+ # Compute baseline triangulation and reprojection error
+ default_p3d = triangulate_simple(subp, cam_mats)
+ default_proj = np.array([project_point_to_cam(default_p3d, M) for M in cam_mats])
+ default_errors = np.linalg.norm(default_proj - subp, axis=1)
+ default_mean_error = np.mean(default_errors)
+
+ # Return immediately if baseline accuracy meets the target
+ if default_mean_error < target_reprojection_error:
+ normalized_camera_weights[:] = 1.0
+ return default_p3d, normalized_camera_weights
+
+ # Initialize best result trackers and combination variables
+ best_p3d, best_error = default_p3d, default_mean_error
+ best_camera_combo = local_indices
+
+ # Set up weighted average sums starting with the baseline triangulation
+ default_weight = np.exp(-5.0 * default_mean_error / target_reprojection_error)
+ weighted_p3d_sum = default_weight * default_p3d
+ total_weight = default_weight
+ normalized_camera_weights[:] += default_weight
+
+ # Iterate through camera combinations by dropping N cameras sequentially
+ for camera_drop_count in range(1, maximum_cameras_to_drop + 1):
+
+ selected_camera_count = total_valid_cams - camera_drop_count
+ if selected_camera_count < minimum_cameras_for_triangulation:
+ break
+
+ combinations = list(itertools.combinations(local_indices, selected_camera_count))
+ for combo in combinations:
+ kept_local_indices = list(combo)
+ candidate_pts = subp[kept_local_indices]
+ candidate_cams = cam_mats[kept_local_indices]
+
+ # Solve triangulation and compute reprojection error for the subset
+ candidate_p3d = triangulate_simple(candidate_pts, candidate_cams)
+ candidate_proj = np.array([
+ project_point_to_cam(candidate_p3d, M) for M in candidate_cams
+ ])
+ candidate_errors = np.linalg.norm(candidate_proj - candidate_pts, axis=1)
+ candidate_mean_error = np.mean(candidate_errors)
+
+ # Accumulate weighted 3D points and per-camera confidence
+ weight = np.exp(-5.0 * candidate_mean_error / target_reprojection_error)
+ weighted_p3d_sum += weight * candidate_p3d
+ total_weight += weight
+ normalized_camera_weights[kept_local_indices] += weight
+
+ # Track the single best-performing combination
+ if candidate_mean_error < best_error:
+ best_error = candidate_mean_error
+ best_p3d = candidate_p3d
+ best_camera_combo = kept_local_indices
+
+ # Stop dropping cameras if target accuracy is achieved at this level
+ if best_error < target_reprojection_error:
+ break
+
+ # Calculate the final weighted 3D point from all tested combinations
+ if total_weight > 1e-12:
+ weighted_p3d = weighted_p3d_sum / total_weight
+ normalized_camera_weights /= total_weight
+ else:
+ # Fallback for numerical underflow from extremely large errors
+ weighted_p3d = best_p3d
+ normalized_camera_weights[:] = 0.0
+ normalized_camera_weights[best_camera_combo] = 1.0
+
+ # Return the weighted_p3d if the error was improved by at least one combination
+ if best_error < default_mean_error:
+ return weighted_p3d, normalized_camera_weights
+ else:
+ normalized_camera_weights[:] = 1.0
+ return default_p3d, normalized_camera_weights
+
+
def get_error_dict(errors_full, min_points=10):
n_cams = errors_full.shape[0]
errors_norm = np.linalg.norm(errors_full, axis=2)
@@ -773,6 +895,76 @@ class CameraGroup:
return out
+ def triangulate_using_outlier_rejection(
+ self,
+ points,
+ undistort=True,
+ progress=False,
+ kill_event: multiprocessing.Event = None,
+ minimum_cameras_for_triangulation: int = 2,
+ maximum_cameras_to_drop: int = 1,
+ target_reprojection_error: float = 0.01,
+ ):
+ """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
+ )
+
+ one_point = False
+ if len(points.shape) == 2:
+ points = points.reshape(-1, 1, 2)
+ one_point = True
+
+ if undistort:
+ new_points = np.empty(points.shape)
+ for cnum, cam in enumerate(self.cameras):
+ # must copy in order to satisfy opencv underneath
+ sub = np.copy(points[cnum])
+ new_points[cnum] = cam.undistort_points(sub)
+ points = new_points
+
+ n_cams, n_points, _ = points.shape
+
+ out = np.empty((n_points, 3))
+ out[:] = np.nan
+
+ normalized_camera_weights = np.zeros((n_points, n_cams), dtype=float)
+
+ cam_mats = np.array([cam.get_extrinsics_mat() for cam in self.cameras])
+
+ if progress:
+ iterator = trange(n_points, ncols=70)
+ else:
+ iterator = range(n_points)
+
+ for ip in iterator:
+ subp = points[:, ip, :]
+ good = ~np.isnan(subp[:, 0])
+ if np.sum(good) >= 2:
+ valid_camera_indices = np.where(good)[0]
+ p3d, weights = triangulate_with_outlier_rejection(
+ subp[good],
+ cam_mats[good],
+ minimum_cameras_for_triangulation=minimum_cameras_for_triangulation,
+ maximum_cameras_to_drop=maximum_cameras_to_drop,
+ target_reprojection_error=target_reprojection_error,
+ )
+ out[ip] = p3d
+ normalized_camera_weights[ip, valid_camera_indices] = weights
+
+ if kill_event is not None and kill_event.is_set():
+ return None, None
+
+ if one_point:
+ out = out[0]
+ normalized_camera_weights = normalized_camera_weights[0]
+
+ return out, normalized_camera_weights
+
def triangulate_possible(
self,
points,
diff --git a/freemocap/core_processes/capture_volume_calibration/by_camera_reprojection_filtering.py b/freemocap/core_processes/capture_volume_calibration/by_camera_reprojection_filtering.py
deleted file mode 100644
index 0aa2a2ca..00000000
--- a/freemocap/core_processes/capture_volume_calibration/by_camera_reprojection_filtering.py
+++ /dev/null
@@ -1,284 +0,0 @@
-import logging
-from pathlib import Path
-from typing import Tuple, Union
-
-import numpy as np
-from matplotlib import pyplot as plt
-
-from freemocap.core_processes.capture_volume_calibration.save_3d_data_to_npy import (
- save_3d_data_to_npy,
-)
-from freemocap.core_processes.capture_volume_calibration.triangulate_3d_data import triangulate_3d_data
-from freemocap.data_layer.recording_models.post_processing_parameter_models import ProcessingParameterModel
-
-logger = logging.getLogger(__name__)
-
-
-def run_reprojection_error_filtering(
- image_data_numCams_numFrames_numTrackedPts_XYZ: np.ndarray,
- raw_skel3d_frame_marker_xyz: np.ndarray,
- skeleton_reprojection_error_cam_fr_mar: np.ndarray,
- skeleton_reprojection_error_fr_mar: np.ndarray,
- anipose_calibration_object,
- processing_parameters: ProcessingParameterModel,
-) -> np.ndarray:
- """
- Runs reprojection error filtering on 3d data, saves the filtered 3d data and reprojection error data, and creates and saves a debug plot.
-
- :param image_data_numCams_numFrames_numTrackedPts_XYZ: original 2d data
- :param raw_skel3d_frame_marker_xyz: original 3d data
- :param skeleton_reprojection_error_cam_fr_mar: original per camerareprojection error
- :param skeleton_reprojection_error_fr_mar: original per frame average reprojection error
- :param anipose_calibration_object: anipose calibration object
- :param processing_parameters: processing parameters
-
- :return: filtered 3d data
- """
- if hasattr(
- processing_parameters.tracking_model_info, "num_tracked_points_body"
- ): # we don't want to reproject hand and face data
- num_tracked_points = processing_parameters.tracking_model_info.num_tracked_points_body
- else:
- num_tracked_points = processing_parameters.tracking_model_info.num_tracked_points
-
- (
- reprojection_filtered_skel3d_frame_marker_xyz,
- reprojection_filtered_skeleton_reprojection_error_fr_mar,
- reprojection_filtered_skeleton_reprojection_error_cam_fr_mar,
- ) = filter_by_reprojection_error(
- reprojection_error_camera_frame_marker=skeleton_reprojection_error_cam_fr_mar,
- reprojection_error_frame_marker=skeleton_reprojection_error_fr_mar,
- reprojection_error_confidence_threshold=processing_parameters.anipose_triangulate_3d_parameters_model.reprojection_error_confidence_cutoff,
- image_2d_data=image_data_numCams_numFrames_numTrackedPts_XYZ[:, :, :, :2],
- raw_skel3d_frame_marker_xyz=raw_skel3d_frame_marker_xyz,
- anipose_calibration_object=anipose_calibration_object,
- num_tracked_points=num_tracked_points,
- use_triangulate_ransac=processing_parameters.anipose_triangulate_3d_parameters_model.use_triangulate_ransac_method,
- minimum_cameras_to_reproject=processing_parameters.anipose_triangulate_3d_parameters_model.minimum_cameras_to_reproject,
- )
- save_3d_data_to_npy(
- data3d_numFrames_numTrackedPoints_XYZ=reprojection_filtered_skel3d_frame_marker_xyz,
- data3d_numFrames_numTrackedPoints_reprojectionError=reprojection_filtered_skeleton_reprojection_error_fr_mar,
- data3d_numCams_numFrames_numTrackedPoints_reprojectionError=reprojection_filtered_skeleton_reprojection_error_cam_fr_mar,
- path_to_folder_where_data_will_be_saved=processing_parameters.recording_info_model.raw_data_folder_path,
- processing_level="reprojection_filtered",
- file_prefix=processing_parameters.tracking_model_info.name,
- )
- plot_reprojection_error(
- raw_reprojection_error_frame_marker=skeleton_reprojection_error_fr_mar,
- filtered_reprojection_error_frame_marker=reprojection_filtered_skeleton_reprojection_error_fr_mar,
- reprojection_error_threshold=float(
- np.nanpercentile(
- skeleton_reprojection_error_cam_fr_mar,
- processing_parameters.anipose_triangulate_3d_parameters_model.reprojection_error_confidence_cutoff,
- )
- ),
- output_folder_path=processing_parameters.recording_info_model.raw_data_folder_path,
- )
- return reprojection_filtered_skel3d_frame_marker_xyz
-
-
-def filter_by_reprojection_error(
- reprojection_error_camera_frame_marker: np.ndarray,
- reprojection_error_frame_marker: np.ndarray,
- reprojection_error_confidence_threshold: float,
- image_2d_data: np.ndarray,
- raw_skel3d_frame_marker_xyz: np.ndarray,
- anipose_calibration_object,
- num_tracked_points: int,
- use_triangulate_ransac: bool = False,
- minimum_cameras_to_reproject: int = 3,
-) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
- total_cameras = image_2d_data.shape[0]
- num_cameras_to_remove = 1
-
- if total_cameras <= minimum_cameras_to_reproject:
- logger.warning(
- f"Not enough cameras to filter by reprojection error. There are {total_cameras} cameras, but minimum number of cameras is {minimum_cameras_to_reproject}. Returning unfiltered data."
- )
- return (
- raw_skel3d_frame_marker_xyz,
- reprojection_error_frame_marker,
- reprojection_error_camera_frame_marker,
- )
-
- body2d_camera_frame_marker_xy = image_2d_data[:, :, :num_tracked_points, :2]
- bodyReprojErr_camera_frame_marker = reprojection_error_camera_frame_marker[:, :, :num_tracked_points]
-
- reprojection_error_confidence_threshold = min(max(reprojection_error_confidence_threshold, 0), 100)
-
- reprojection_error_threshold = np.nanpercentile(
- np.nanmean(bodyReprojErr_camera_frame_marker, axis=0), reprojection_error_confidence_threshold, method="weibull"
- ) # TODO: try running this on reprojection_error_frame_marker with body points pulled out, rather than with cameras included
- logger.info(f"Using reprojection error threshold of {reprojection_error_threshold}")
-
- data_to_reproject_camera_frame_marker_xy, unique_frame_marker_list = _get_data_to_reproject(
- num_cameras_to_remove=num_cameras_to_remove,
- reprojection_error_threshold=reprojection_error_threshold,
- reprojError_cam_frame_marker=bodyReprojErr_camera_frame_marker,
- input_2d_data_camera_frame_marker_xy=body2d_camera_frame_marker_xy,
- )
-
- while len(unique_frame_marker_list) > 0 and total_cameras - num_cameras_to_remove >= minimum_cameras_to_reproject:
- logger.info("Retriangulating data")
- (
- retriangulated_data_frame_marker_xyz,
- new_reprojection_error_flat,
- new_reprojError_cam_frame_marker,
- ) = triangulate_3d_data(
- anipose_calibration_object=anipose_calibration_object,
- image_2d_data=data_to_reproject_camera_frame_marker_xy,
- use_triangulate_ransac=use_triangulate_ransac,
- )
-
- num_cameras_to_remove += 1
-
- data_to_reproject_camera_frame_marker_xy, unique_frame_marker_list = _get_data_to_reproject(
- num_cameras_to_remove=num_cameras_to_remove,
- reprojection_error_threshold=reprojection_error_threshold,
- reprojError_cam_frame_marker=new_reprojError_cam_frame_marker,
- input_2d_data_camera_frame_marker_xy=data_to_reproject_camera_frame_marker_xy,
- )
-
- # nan remaining data above threshold
- _nan_data_above_threshold(unique_frame_marker_list, retriangulated_data_frame_marker_xyz)
-
- # put retriangulated data back in place
- filtered_skel3d_frame_marker_xyz = raw_skel3d_frame_marker_xyz.copy()
- filtered_skel3d_frame_marker_xyz[:, :num_tracked_points, :] = retriangulated_data_frame_marker_xyz
-
- filtered_reprojection_error_frame_marker = reprojection_error_frame_marker.copy()
- filtered_reprojection_error_frame_marker[:, :num_tracked_points] = new_reprojection_error_flat
-
- filtered_reprojection_error_camera_frame_marker = reprojection_error_camera_frame_marker.copy()
- filtered_reprojection_error_camera_frame_marker[:, :, :num_tracked_points] = new_reprojError_cam_frame_marker
-
- return (
- filtered_skel3d_frame_marker_xyz,
- filtered_reprojection_error_frame_marker,
- filtered_reprojection_error_camera_frame_marker,
- )
-
-
-def _nan_data_above_threshold(unique_frame_marker_list: list, retriangulated_data_frame_marker_xyz: np.ndarray) -> None:
- if len(unique_frame_marker_list) > 0:
- logger.info(
- f"Out of cameras to remove, setting {len(unique_frame_marker_list)} points with reprojection error above threshold to NaNs"
- )
- for frame_marker in unique_frame_marker_list:
- retriangulated_data_frame_marker_xyz[frame_marker[0], frame_marker[1], :] = np.nan
-
-
-def _get_data_to_reproject(
- num_cameras_to_remove: int,
- reprojection_error_threshold: float,
- reprojError_cam_frame_marker: np.ndarray,
- input_2d_data_camera_frame_marker_xy: np.ndarray,
-) -> tuple[np.ndarray, list]:
- indices_above_threshold = np.nonzero(reprojError_cam_frame_marker > reprojection_error_threshold)
- logger.debug(f"SHAPE OF INDICES ABOVE THRESHOLD: {indices_above_threshold[0].shape}")
-
- total_frame_marker_combos = (
- input_2d_data_camera_frame_marker_xy.shape[1] * input_2d_data_camera_frame_marker_xy.shape[2]
- )
- unique_frame_marker_list = _get_unique_frame_marker_list(indices_above_threshold=indices_above_threshold)
- logger.info(
- f"number of frame/marker combos with reprojection error above threshold: {len(unique_frame_marker_list)} ({len(unique_frame_marker_list) / total_frame_marker_combos * 100:.1f} percent of total)"
- )
-
- cameras_to_remove, frames_to_reproject, markers_to_reproject = _get_camera_frame_marker_lists_to_reproject(
- reprojError_cam_frame_marker=reprojError_cam_frame_marker,
- frame_marker_list=unique_frame_marker_list,
- num_cameras_to_remove=num_cameras_to_remove,
- )
-
- data_to_reproject_camera_frame_marker_xy = _set_unincluded_data_to_nans(
- input_2d_data=input_2d_data_camera_frame_marker_xy,
- frames_with_reprojection_error=frames_to_reproject,
- markers_with_reprojection_error=markers_to_reproject,
- cameras_to_remove=cameras_to_remove,
- )
-
- return (data_to_reproject_camera_frame_marker_xy, unique_frame_marker_list)
-
-
-def _get_unique_frame_marker_list(
- indices_above_threshold: np.ndarray,
-) -> list:
- return list(set(zip(indices_above_threshold[1], indices_above_threshold[2])))
-
-
-def _get_camera_frame_marker_lists_to_reproject(
- reprojError_cam_frame_marker: np.ndarray,
- frame_marker_list: list,
- num_cameras_to_remove: int,
-) -> Tuple[list, list, list]:
- """
- Generate the lists of cameras, frames, and markers to reproject based on the given input.
- Find the cameras with the worst reprojection errors for the given frames and markers.
- Args:
- reprojError_cam_frame_marker (np.ndarray): The array containing the reprojection errors for each camera, frame, and marker.
- frame_marker_list (list): The list of tuples containing the frame and marker indices.
- num_cameras_to_remove (int): The number of cameras to remove.
- Returns:
- Tuple[list, list, list]: A tuple containing the lists of cameras to remove, frames to reproject, and markers to reproject.
- """
- cameras_to_remove = []
- frames_to_reproject = []
- markers_to_reproject = []
- for frame, marker in frame_marker_list:
- frames_to_reproject.append(frame)
- markers_to_reproject.append(marker)
- max_indices = reprojError_cam_frame_marker[:, frame, marker].argsort()[::-1][:num_cameras_to_remove]
- cameras_to_remove.append(list(max_indices))
- return (cameras_to_remove, frames_to_reproject, markers_to_reproject)
-
-
-def _set_unincluded_data_to_nans(
- input_2d_data: np.ndarray,
- frames_with_reprojection_error: list,
- markers_with_reprojection_error: list,
- cameras_to_remove: list[list[int]],
-) -> np.ndarray:
- data_to_reproject = input_2d_data.copy()
- for list_of_cameras, frame, marker in zip(
- cameras_to_remove, frames_with_reprojection_error, markers_with_reprojection_error
- ):
- for camera in list_of_cameras:
- data_to_reproject[camera, frame, marker, :] = np.nan
- return data_to_reproject
-
-
-def plot_reprojection_error(
- raw_reprojection_error_frame_marker: np.ndarray,
- filtered_reprojection_error_frame_marker: np.ndarray,
- reprojection_error_threshold: float,
- output_folder_path: Union[str, Path],
-) -> None:
- title = "Mean Reprojection Error Per Frame"
- file_name = "debug_reprojection_error_filtering.png"
- output_filepath = Path(output_folder_path) / file_name
- raw_mean_reprojection_error_per_frame = np.nanmean(
- raw_reprojection_error_frame_marker,
- axis=1,
- )
- filtered_mean_reprojection_error_per_frame = np.nanmean(
- filtered_reprojection_error_frame_marker,
- axis=1,
- )
- plt.plot(raw_mean_reprojection_error_per_frame, color="blue", label="Data Before Filtering")
- plt.plot(filtered_mean_reprojection_error_per_frame, color="orange", alpha=0.9, label="Data After Filtering")
- plt.xlabel("Frame")
- plt.ylabel("Mean Reprojection Error Across Markers (mm)")
- plt.ylim(0, 2 * reprojection_error_threshold)
- plt.hlines(
- y=reprojection_error_threshold,
- xmin=0,
- xmax=len(raw_mean_reprojection_error_per_frame),
- color="red",
- label="Cutoff Threshold",
- )
- plt.title(title)
- plt.legend(loc="upper right")
- logger.info(f"Saving debug plots to: {output_filepath}")
- plt.savefig(output_filepath, dpi=300)
diff --git a/freemocap/core_processes/capture_volume_calibration/save_3d_data_to_npy.py b/freemocap/core_processes/capture_volume_calibration/save_3d_data_to_npy.py
index c82ffc2f..57672db2 100644
--- a/freemocap/core_processes/capture_volume_calibration/save_3d_data_to_npy.py
+++ b/freemocap/core_processes/capture_volume_calibration/save_3d_data_to_npy.py
@@ -8,7 +8,6 @@ from freemocap.system.paths_and_filenames.file_and_folder_names import (
RAW_3D_NPY_FILE_NAME,
FULL_REPROJECTION_ERROR_NPY_FILE_NAME,
REPROJECTION_ERROR_NPY_FILE_NAME,
- REPROJECTION_FILTERED_PREFIX,
)
logger = logging.getLogger(__name__)
@@ -19,35 +18,17 @@ def save_3d_data_to_npy(
data3d_numFrames_numTrackedPoints_reprojectionError: np.ndarray,
data3d_numCams_numFrames_numTrackedPoints_reprojectionError: np.ndarray,
path_to_folder_where_data_will_be_saved: Union[str, Path],
- processing_level: str,
file_prefix: str = "",
):
path_to_folder_where_data_will_be_saved = Path(path_to_folder_where_data_will_be_saved)
Path(path_to_folder_where_data_will_be_saved).mkdir(parents=True, exist_ok=True) # save spatial XYZ data
- if processing_level == "raw":
- data_3d_save_path = path_to_folder_where_data_will_be_saved / f"{file_prefix}_{RAW_3D_NPY_FILE_NAME}"
- reprojection_error_save_path = (
- path_to_folder_where_data_will_be_saved / f"{file_prefix}_{REPROJECTION_ERROR_NPY_FILE_NAME}"
- )
- full_reprojection_error_save_path = (
- path_to_folder_where_data_will_be_saved / f"{file_prefix}_{FULL_REPROJECTION_ERROR_NPY_FILE_NAME}"
- )
- elif processing_level == "reprojection_filtered":
- data_3d_save_path = (
- path_to_folder_where_data_will_be_saved
- / f"{REPROJECTION_FILTERED_PREFIX}{file_prefix}_{RAW_3D_NPY_FILE_NAME}"
- )
- reprojection_error_save_path = (
- path_to_folder_where_data_will_be_saved
- / f"{REPROJECTION_FILTERED_PREFIX}{file_prefix}_{REPROJECTION_ERROR_NPY_FILE_NAME}"
- )
- full_reprojection_error_save_path = (
- path_to_folder_where_data_will_be_saved
- / f"{REPROJECTION_FILTERED_PREFIX}{file_prefix}_{FULL_REPROJECTION_ERROR_NPY_FILE_NAME}"
- )
- else:
- logger.exception(f"processing_level: {processing_level} not recognized")
- raise Exception
+ data_3d_save_path = path_to_folder_where_data_will_be_saved / f"{file_prefix}_{RAW_3D_NPY_FILE_NAME}"
+ reprojection_error_save_path = (
+ path_to_folder_where_data_will_be_saved / f"{file_prefix}_{REPROJECTION_ERROR_NPY_FILE_NAME}"
+ )
+ full_reprojection_error_save_path = (
+ path_to_folder_where_data_will_be_saved / f"{file_prefix}_{FULL_REPROJECTION_ERROR_NPY_FILE_NAME}"
+ )
logger.info(f"saving: {data_3d_save_path}")
np.save(str(data_3d_save_path), data3d_numFrames_numTrackedPoints_XYZ)
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 c8ba40ee..876f5a86 100644
--- a/freemocap/core_processes/capture_volume_calibration/triangulate_3d_data.py
+++ b/freemocap/core_processes/capture_volume_calibration/triangulate_3d_data.py
@@ -1,5 +1,6 @@
import logging
import multiprocessing
+from typing import Optional
from pathlib import Path
import numpy as np
@@ -13,11 +14,15 @@ logger = logging.getLogger(__name__)
def triangulate_3d_data(
- anipose_calibration_object: CameraGroup,
- image_2d_data: np.ndarray,
- use_triangulate_ransac: bool = False,
- kill_event: multiprocessing.Event = None,
-) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
+ anipose_calibration_object: CameraGroup,
+ image_2d_data: np.ndarray,
+ use_triangulate_ransac: bool = False,
+ use_triangulate_outlier_rejection: bool = False,
+ minimum_cameras_for_triangulation: int = 2,
+ maximum_cameras_to_drop: int = 1,
+ target_reprojection_error: float = 0.01,
+ kill_event: multiprocessing.Event = None,
+) -> tuple[np.ndarray, np.ndarray, np.ndarray, Optional[np.ndarray]]:
number_of_cameras = image_2d_data.shape[0]
number_of_frames = image_2d_data.shape[1]
number_of_tracked_points = image_2d_data.shape[2]
@@ -41,9 +46,20 @@ def triangulate_3d_data(
f"number_of_spatial_dimensions: {number_of_spatial_dimensions}"
)
- if use_triangulate_ransac:
- logger.info("Using `triangulate_ransac` method")
- data3d_flat = anipose_calibration_object.triangulate_ransac(data2d_flat, progress=True, kill_event=kill_event)
+ normalized_camera_weights = None
+ if use_triangulate_outlier_rejection:
+ logger.info("Using `triangulate_using_outlier_rejection` method")
+ data3d_flat, normalized_camera_weights_flat = anipose_calibration_object.triangulate_using_outlier_rejection(
+ data2d_flat,
+ progress=True,
+ kill_event=kill_event,
+ minimum_cameras_for_triangulation=minimum_cameras_for_triangulation,
+ maximum_cameras_to_drop=maximum_cameras_to_drop,
+ target_reprojection_error=target_reprojection_error,
+ )
+ normalized_camera_weights = normalized_camera_weights_flat.reshape(
+ number_of_frames, number_of_tracked_points, number_of_cameras
+ )
else:
logger.info("Using simple `triangulate` method ")
data3d_flat = anipose_calibration_object.triangulate(data2d_flat, progress=True, kill_event=kill_event)
@@ -64,6 +80,7 @@ def triangulate_3d_data(
spatial_data3d_numFrames_numTrackedPoints_XYZ,
reprojection_error_data3d_numFrames_numTrackedPoints,
reprojectionError_cam_frame_marker,
+ normalized_camera_weights,
)
@@ -99,9 +116,27 @@ if __name__ == "__main__":
required=False,
)
parser.add_argument(
- "--use_triangulate_ransac",
+ "--use_triangulate_outlier_rejection",
type=bool,
- help="use triangulate ransac anipose method ",
+ help="use triangulate using outlier rejection anipose method ",
+ required=False,
+ )
+ parser.add_argument(
+ "--minimum_cameras_for_triangulation",
+ type=int,
+ help="minimum number of cameras required for triangulation",
+ required=False,
+ )
+ parser.add_argument(
+ "--maximum_cameras_to_drop",
+ type=int,
+ help="maximum amount of cameras permitted to drop",
+ required=False,
+ )
+ parser.add_argument(
+ "--target_reprojection_error",
+ type=float,
+ help="maximum acceptable mean reprojection error",
required=False,
)
parser.add_argument(
@@ -120,8 +155,17 @@ if __name__ == "__main__":
if args.save_data_as_csv is None:
args.save_data_as_csv = True # default
- if args.use_triangulate_ransac is None:
- args.use_triangulate_ransac = True
+ if args.use_triangulate_outlier_rejection is None:
+ args.use_triangulate_outlier_rejection = False
+
+ if args.minimum_cameras_for_triangulation is None:
+ args.minimum_cameras_for_triangulation = 2
+
+ if args.maximum_cameras_to_drop is None:
+ args.maximum_cameras_to_drop = 1
+
+ if args.target_reprojection_error is None:
+ args.target_reprojection_error = 0.01
if args.file_prefix is None:
args.file_prefix = ""
@@ -137,16 +181,20 @@ if __name__ == "__main__":
skel3d_frame_marker_xyz,
skeleton_reprojection_error_fr_mar,
skeleton_reprojection_error_cam_fr_mar,
+ normalized_camera_weights,
) = triangulate_3d_data(
anipose_calibration_object=anipose_calibration_object,
image_2d_data=data_2d,
- use_triangulate_ransac=args.use_triangulate_ransac,
+ use_triangulate_ransac=False,
+ use_triangulate_outlier_rejection=args.use_triangulate_outlier_rejection,
+ minimum_cameras_for_triangulation=args.minimum_cameras_for_triangulation,
+ maximum_cameras_to_drop=args.maximum_cameras_to_drop,
+ target_reprojection_error=args.target_reprojection_error,
)
save_3d_data_to_npy(
data3d_numFrames_numTrackedPoints_XYZ=skel3d_frame_marker_xyz,
data3d_numFrames_numTrackedPoints_reprojectionError=skeleton_reprojection_error_fr_mar,
data3d_numCams_numFrames_numTrackedPoints_reprojectionError=skeleton_reprojection_error_cam_fr_mar,
path_to_folder_where_data_will_be_saved=args.output_data_folder_path,
- processing_level="raw",
file_prefix=args.file_prefix,
)
diff --git a/freemocap/core_processes/post_process_skeleton_data/process_single_camera_skeleton_data.py b/freemocap/core_processes/post_process_skeleton_data/process_single_camera_skeleton_data.py
index ff9bce39..71a39a8f 100644
--- a/freemocap/core_processes/post_process_skeleton_data/process_single_camera_skeleton_data.py
+++ b/freemocap/core_processes/post_process_skeleton_data/process_single_camera_skeleton_data.py
@@ -37,7 +37,6 @@ def process_single_camera_skeleton_data(
data3d_numFrames_numTrackedPoints_reprojectionError=skeleton_reprojection_error_fr_mar,
data3d_numCams_numFrames_numTrackedPoints_reprojectionError=skeleton_reprojection_error_cam_fr_mar,
path_to_folder_where_data_will_be_saved=raw_data_folder_path,
- processing_level="raw",
file_prefix=file_prefix,
)
rotated_raw_skel3d_frame_marker_xyz = rotate_by_90_degrees_around_x_axis(raw_skel3d_frame_marker_xyz)
diff --git a/freemocap/core_processes/process_motion_capture_videos/processing_pipeline_functions/triangulation_pipeline_functions.py b/freemocap/core_processes/process_motion_capture_videos/processing_pipeline_functions/triangulation_pipeline_functions.py
index 3782fe55..b9019b1b 100644
--- a/freemocap/core_processes/process_motion_capture_videos/processing_pipeline_functions/triangulation_pipeline_functions.py
+++ b/freemocap/core_processes/process_motion_capture_videos/processing_pipeline_functions/triangulation_pipeline_functions.py
@@ -8,9 +8,6 @@ import numpy as np
from freemocap.core_processes.capture_volume_calibration.anipose_camera_calibration.get_anipose_calibration_object import (
load_anipose_calibration_toml_from_path,
)
-from freemocap.core_processes.capture_volume_calibration.by_camera_reprojection_filtering import (
- run_reprojection_error_filtering,
-)
from freemocap.core_processes.capture_volume_calibration.save_3d_data_to_npy import (
save_3d_data_to_npy,
)
@@ -79,10 +76,15 @@ def get_triangulated_data(
skel3d_frame_marker_xyz,
skeleton_reprojection_error_fr_mar,
skeleton_reprojection_error_cam_fr_mar,
+ normalized_camera_weights,
) = triangulate_3d_data(
anipose_calibration_object=anipose_calibration_object,
image_2d_data=image_data_numCams_numFrames_numTrackedPts_XYZ[:, :, :, :2],
- use_triangulate_ransac=processing_parameters.anipose_triangulate_3d_parameters_model.use_triangulate_ransac_method,
+ use_triangulate_ransac=False,
+ use_triangulate_outlier_rejection=processing_parameters.anipose_triangulate_3d_parameters_model.use_triangulate_outlier_rejection,
+ minimum_cameras_for_triangulation=processing_parameters.anipose_triangulate_3d_parameters_model.minimum_cameras_for_triangulation,
+ maximum_cameras_to_drop=processing_parameters.anipose_triangulate_3d_parameters_model.maximum_cameras_to_drop,
+ target_reprojection_error=processing_parameters.anipose_triangulate_3d_parameters_model.target_reprojection_error,
kill_event=kill_event,
)
save_3d_data_to_npy(
@@ -90,19 +92,13 @@ def get_triangulated_data(
data3d_numFrames_numTrackedPoints_reprojectionError=skeleton_reprojection_error_fr_mar,
data3d_numCams_numFrames_numTrackedPoints_reprojectionError=skeleton_reprojection_error_cam_fr_mar,
path_to_folder_where_data_will_be_saved=processing_parameters.recording_info_model.raw_data_folder_path,
- processing_level="raw",
file_prefix=processing_parameters.tracking_model_info.name,
)
- if processing_parameters.anipose_triangulate_3d_parameters_model.run_reprojection_error_filtering:
- logger.info("Filtering 3d triangulation...")
- skel3d_frame_marker_xyz = run_reprojection_error_filtering(
- image_data_numCams_numFrames_numTrackedPts_XYZ=image_data_numCams_numFrames_numTrackedPts_XYZ,
- raw_skel3d_frame_marker_xyz=skel3d_frame_marker_xyz,
- skeleton_reprojection_error_fr_mar=skeleton_reprojection_error_fr_mar,
- skeleton_reprojection_error_cam_fr_mar=skeleton_reprojection_error_cam_fr_mar,
- anipose_calibration_object=anipose_calibration_object,
- processing_parameters=processing_parameters,
- )
+ if processing_parameters.anipose_triangulate_3d_parameters_model.use_triangulate_outlier_rejection and normalized_camera_weights is not None:
+ weights_filename = f"{processing_parameters.tracking_model_info.name}_outlier_rejection_normalized_camera_weights.npy"
+ weights_filepath = Path(processing_parameters.recording_info_model.raw_data_folder_path) / weights_filename
+ logger.info(f"Saving normalized camera weights to {weights_filepath}")
+ np.save(weights_filepath, normalized_camera_weights)
return skel3d_frame_marker_xyz
diff --git a/freemocap/data_layer/data_saver/data_loader.py b/freemocap/data_layer/data_saver/data_loader.py
index 703ee3de..68f5f030 100644
--- a/freemocap/data_layer/data_saver/data_loader.py
+++ b/freemocap/data_layer/data_saver/data_loader.py
@@ -22,7 +22,6 @@ from freemocap.system.paths_and_filenames.file_and_folder_names import (
RAW_DATA_FOLDER_NAME,
REPROJECTION_ERROR_NPY_FILE_NAME,
FULL_REPROJECTION_ERROR_NPY_FILE_NAME,
- REPROJECTION_FILTERED_PREFIX,
)
from freemocap.system.paths_and_filenames.path_getters import (
get_output_data_folder_path,
@@ -161,33 +160,16 @@ class DataLoader:
def _load_reprojection_error_data(self):
"""
Load reprojection error averaged across cameras and by camera.
- Defaults to using reprojection filtered values if they exist.
"""
- filtered_average_error_path = (
- self._raw_data_folder_path
- / f"{REPROJECTION_FILTERED_PREFIX}{self._file_prefix}{REPROJECTION_ERROR_NPY_FILE_NAME}"
- )
- filtered_by_camera_path = (
- self._raw_data_folder_path
- / f"{REPROJECTION_FILTERED_PREFIX}{self._file_prefix}{FULL_REPROJECTION_ERROR_NPY_FILE_NAME}"
- )
raw_average_error_path = self._raw_data_folder_path / f"{self._file_prefix}{REPROJECTION_ERROR_NPY_FILE_NAME}"
raw_by_camera_path = self._raw_data_folder_path / f"{self._file_prefix}{FULL_REPROJECTION_ERROR_NPY_FILE_NAME}"
try:
- if filtered_average_error_path.exists():
- logger.debug(f"Loading filtered average reprojection error from {filtered_average_error_path}")
- self.reprojection_error_frame_name_value = np.load(filtered_average_error_path)
- else:
- logger.debug(f"Loading unfiltered average reprojection error from {raw_average_error_path}")
- self.reprojection_error_frame_name_value = np.load(raw_average_error_path)
- if filtered_by_camera_path.exists():
- logger.debug(f"Loading filtered by camera reprojection error from {filtered_by_camera_path}")
- self.reprojection_error_camera_frame_name_value = np.load(filtered_by_camera_path)
- else:
- logger.debug(f"Loading unfiltered by camera reprojection error from {raw_by_camera_path}")
- self.reprojection_error_camera_frame_name_value = np.load(raw_by_camera_path)
+ logger.debug(f"Loading average reprojection error from {raw_average_error_path}")
+ self.reprojection_error_frame_name_value = np.load(raw_average_error_path)
+ logger.debug(f"Loading by camera reprojection error from {raw_by_camera_path}")
+ self.reprojection_error_camera_frame_name_value = np.load(raw_by_camera_path)
except FileNotFoundError:
logger.warning("unable to load reprojection error data from file.")
self.reprojection_error_frame_name_value = None
diff --git a/freemocap/data_layer/recording_models/post_processing_parameter_models.py b/freemocap/data_layer/recording_models/post_processing_parameter_models.py
index 1260bb10..5e67cf07 100644
--- a/freemocap/data_layer/recording_models/post_processing_parameter_models.py
+++ b/freemocap/data_layer/recording_models/post_processing_parameter_models.py
@@ -13,10 +13,11 @@ logger = logging.getLogger(__name__)
class AniposeTriangulate3DParametersModel(BaseModel):
- run_reprojection_error_filtering: bool = False
- reprojection_error_confidence_cutoff: float = 90
- minimum_cameras_to_reproject: int = 3
use_triangulate_ransac_method: bool = False
+ use_triangulate_outlier_rejection: bool = False
+ minimum_cameras_for_triangulation: int = 2
+ maximum_cameras_to_drop: int = 1
+ target_reprojection_error: float = 0.01
run_3d_triangulation: bool = True
flatten_single_camera_data: bool = True
diff --git a/freemocap/gui/qt/widgets/control_panel/process_mocap_data_panel/parameter_groups/create_parameter_groups.py b/freemocap/gui/qt/widgets/control_panel/process_mocap_data_panel/parameter_groups/create_parameter_groups.py
index b00005b2..830d582b 100644
--- a/freemocap/gui/qt/widgets/control_panel/process_mocap_data_panel/parameter_groups/create_parameter_groups.py
+++ b/freemocap/gui/qt/widgets/control_panel/process_mocap_data_panel/parameter_groups/create_parameter_groups.py
@@ -18,17 +18,17 @@ POST_PROCESSING_FRAME_RATE = "Framerate"
BUTTERWORTH_FILTER_TREE_NAME = "Butterworth Filter"
-USE_RANSAC_METHOD = "Use RANSAC Method"
+OUTLIER_REJECTION_TREE_NAME = "Outlier Rejection"
-ANIPOSE_CONFIDENCE_CUTOFF = "Confidence Threshold Cut-off"
+USE_OUTLIER_REJECTION_METHOD = "Use Outlier Rejection Method?"
-REPROJECTION_ERROR_FILTERING_TREE_NAME = "Reprojection Error Filtering"
+OUTLIER_REJECTION_MAXIMUM_CAMERAS_TO_DROP = "Maximum Cameras to Drop"
-RUN_REPROJECTION_ERROR_FILTERING = "Run Reprojection Error Filtering"
+OUTLIER_REJECTION_MINIMUM_CAMERAS_FOR_TRIANGULATION = "Minimum Cameras for Triangulation"
-REPROJECTION_ERROR_FILTER_THRESHOLD = "Reprojection Error Filter Threshold (%)"
+OUTLIER_REJECTION_TARGET_REPROJECTION_ERROR = "Target Reprojection Error"
-MINIMUM_CAMERAS_TO_REPROJECT = "Minimum Cameras to Reproject"
+ANIPOSE_CONFIDENCE_CUTOFF = "Confidence Threshold Cut-off"
FLATTEN_SINGLE_CAMERA_DATA = "Flatten Single Camera Data (Recommended)"
@@ -159,13 +159,6 @@ def create_3d_triangulation_parameter_group(
name=ANIPOSE_TREE_NAME,
type="group",
children=[
- dict(
- name=USE_RANSAC_METHOD,
- type="bool",
- value=parameter_model.use_triangulate_ransac_method,
- tip="If true, use `anipose`'s `triangulate_ransac` method instead of the default `triangulate_simple` method. "
- "NOTE - Much slower than the 'simple' method, but might be more accurate and better at rejecting bad camera views. Needs more testing and evaluation to see if it's worth it. ",
- ),
dict(
name=FLATTEN_SINGLE_CAMERA_DATA,
type="bool",
@@ -173,26 +166,39 @@ def create_3d_triangulation_parameter_group(
tip="If true, flatten the data from single camera recordings.",
),
dict(
- name=REPROJECTION_ERROR_FILTERING_TREE_NAME,
+ name=OUTLIER_REJECTION_TREE_NAME,
type="group",
children=[
dict(
- name=RUN_REPROJECTION_ERROR_FILTERING,
+ name=USE_OUTLIER_REJECTION_METHOD,
type="bool",
- value=parameter_model.run_reprojection_error_filtering,
- tip="If true, run filtering of reprojection error.",
+ value=parameter_model.use_triangulate_outlier_rejection,
+ tip="If true, use `anipose`'s `triangulate_using_outlier_rejection` method.",
),
dict(
- name=REPROJECTION_ERROR_FILTER_THRESHOLD,
- type="float",
- value=parameter_model.reprojection_error_confidence_cutoff,
- tip="The maximum reprojection error allowed in the data.",
+ name=OUTLIER_REJECTION_MAXIMUM_CAMERAS_TO_DROP,
+ type="int",
+ value=parameter_model.maximum_cameras_to_drop,
+ limits=(0, 100),
+ step=1,
+ tip="Maximum amount of cameras permitted to drop.",
),
dict(
- name=MINIMUM_CAMERAS_TO_REPROJECT,
+ name=OUTLIER_REJECTION_MINIMUM_CAMERAS_FOR_TRIANGULATION,
type="int",
- value=parameter_model.minimum_cameras_to_reproject,
- tip="The minimum number of cameras to reproject during retriangulation.",
+ value=parameter_model.minimum_cameras_for_triangulation,
+ limits=(1, 100),
+ step=1,
+ tip="Minimum number of cameras required for triangulation.",
+ ),
+ dict(
+ name=OUTLIER_REJECTION_TARGET_REPROJECTION_ERROR,
+ type="float",
+ value=parameter_model.target_reprojection_error,
+ limits=(0.0, 1.0),
+ step=0.001,
+ tip="The target reprojection error that stops the outlier rejection search.\n"
+ "If a camera combination achieves an error below this value, it is accepted and further dropped-camera iterations are skipped.",
),
],
),
@@ -257,11 +263,11 @@ def extract_parameter_model_from_parameter_tree(
bounding_box_buffer_percentage=parameter_values_dictionary[BOUNDING_BOX_BUFFER_PERCENTAGE],
),
anipose_triangulate_3d_parameters_model=AniposeTriangulate3DParametersModel(
- run_reprojection_error_filtering=parameter_values_dictionary[RUN_REPROJECTION_ERROR_FILTERING],
- reprojection_error_confidence_cutoff=parameter_values_dictionary[REPROJECTION_ERROR_FILTER_THRESHOLD],
- minimum_cameras_to_reproject=parameter_values_dictionary[MINIMUM_CAMERAS_TO_REPROJECT],
- use_triangulate_ransac_method=parameter_values_dictionary[USE_RANSAC_METHOD],
flatten_single_camera_data=parameter_values_dictionary[FLATTEN_SINGLE_CAMERA_DATA],
+ use_triangulate_outlier_rejection=parameter_values_dictionary[USE_OUTLIER_REJECTION_METHOD],
+ minimum_cameras_for_triangulation=parameter_values_dictionary[OUTLIER_REJECTION_MINIMUM_CAMERAS_FOR_TRIANGULATION],
+ maximum_cameras_to_drop=parameter_values_dictionary[OUTLIER_REJECTION_MAXIMUM_CAMERAS_TO_DROP],
+ target_reprojection_error=parameter_values_dictionary[OUTLIER_REJECTION_TARGET_REPROJECTION_ERROR],
run_3d_triangulation=parameter_values_dictionary[RUN_3D_TRIANGULATION_NAME],
),
post_processing_parameters_model=PostProcessingParametersModel(
diff --git a/freemocap/system/paths_and_filenames/file_and_folder_names.py b/freemocap/system/paths_and_filenames/file_and_folder_names.py
index 6c120461..37f83a77 100644
--- a/freemocap/system/paths_and_filenames/file_and_folder_names.py
+++ b/freemocap/system/paths_and_filenames/file_and_folder_names.py
@@ -26,7 +26,7 @@ DATA_2D_NPY_FILE_NAME = "2dData_numCams_numFrames_numTrackedPoints_pixelXY.npy"
RAW_3D_NPY_FILE_NAME = "3dData_numFrames_numTrackedPoints_spatialXYZ.npy"
REPROJECTION_ERROR_NPY_FILE_NAME = "3dData_numFrames_numTrackedPoints_reprojectionError.npy"
FULL_REPROJECTION_ERROR_NPY_FILE_NAME = "3dData_numCams_numFrames_numTrackedPoints_reprojectionError.npy"
-REPROJECTION_FILTERED_PREFIX = "reprojection_filtered_"
+
DATA_3D_NPY_FILE_NAME = "skeleton_3d.npy"
RIGID_BONES_NPY_FILE_NAME = "rigid_bones_3d.npy"
diff --git a/freemocap/tests/test_by_camera_reprojection_filtering.py b/freemocap/tests/test_by_camera_reprojection_filtering.py
deleted file mode 100644
index 22e48411..00000000
--- a/freemocap/tests/test_by_camera_reprojection_filtering.py
+++ /dev/null
@@ -1,96 +0,0 @@
-import numpy as np
-
-from freemocap.core_processes.capture_volume_calibration.by_camera_reprojection_filtering import (
- _get_camera_frame_marker_lists_to_reproject,
- _set_unincluded_data_to_nans,
-)
-
-
-def test_get_camera_frame_marker_lists_to_reproject():
- # test removing one camera
- reprojError_cam_frame_marker = np.array(
- [[[1, 5, 3], [4, 5, 6], [7, 8, 9]], [[2, 3, 4], [5, 6, 9], [8, 9, 10]], [[3, 6, 5], [6, 7, 8], [9, 10, 11]]]
- )
- frame_marker_list = [(0, 1), (1, 2)]
- num_cameras_to_remove = 1
- expected_output = ([[2], [1]], [0, 1], [1, 2])
- assert (
- _get_camera_frame_marker_lists_to_reproject(
- reprojError_cam_frame_marker, frame_marker_list, num_cameras_to_remove
- )
- == expected_output
- )
- # test removing two cameras
- frame_marker_list = [(0, 1), (1, 2)]
- num_cameras_to_remove = 2
- expected_output = ([[2, 0], [1, 2]], [0, 1], [1, 2])
- assert (
- _get_camera_frame_marker_lists_to_reproject(
- reprojError_cam_frame_marker, frame_marker_list, num_cameras_to_remove
- )
- == expected_output
- )
- # test removing all cameras
- frame_marker_list = [(0, 1), (1, 2)]
- num_cameras_to_remove = 3
- expected_output = ([[2, 0, 1], [1, 2, 0]], [0, 1], [1, 2])
- assert (
- _get_camera_frame_marker_lists_to_reproject(
- reprojError_cam_frame_marker, frame_marker_list, num_cameras_to_remove
- )
- == expected_output
- )
- # test removing more cameras than dimensions
- frame_marker_list = [(0, 1), (1, 2)]
- num_cameras_to_remove = 4
- # should just return all of the camera sorted by max reprojection error, no need to error
- expected_output = ([[2, 0, 1], [1, 2, 0]], [0, 1], [1, 2])
- assert (
- _get_camera_frame_marker_lists_to_reproject(
- reprojError_cam_frame_marker, frame_marker_list, num_cameras_to_remove
- )
- == expected_output
- )
- # test providing no frames or markers
- frame_marker_list = []
- num_cameras_to_remove = 2
- expected_output = ([], [], [])
- assert (
- _get_camera_frame_marker_lists_to_reproject(
- reprojError_cam_frame_marker, frame_marker_list, num_cameras_to_remove
- )
- == expected_output
- )
-
-
-def test_set_unincluded_data_to_nans():
- # test indices are turned to nans
- data = np.random.rand(5, 5, 5, 2)
- frames_with_reprojection_error = [1, 2]
- markers_with_reprojection_error = [3, 4]
- cameras_to_remove = [[0, 1], [2, 3]]
- result = _set_unincluded_data_to_nans(data, frames_with_reprojection_error, markers_with_reprojection_error,
- cameras_to_remove)
- for cameras, frame, marker in zip(cameras_to_remove, frames_with_reprojection_error,
- markers_with_reprojection_error):
- for camera in cameras:
- np.testing.assert_array_equal(result[camera, frame, marker, :], np.full((2,), np.nan))
-
- # test other indices aren't touched
- data = np.random.rand(5, 5, 5, 2)
- frames_with_reprojection_error = [1, 2]
- markers_with_reprojection_error = [3, 4]
- cameras_to_remove = [[0, 1], [2, 3]]
- original_data = data.copy()
- result = _set_unincluded_data_to_nans(data, frames_with_reprojection_error, markers_with_reprojection_error,
- cameras_to_remove)
- mask = np.ones(data.shape, dtype=bool)
- for cameras, frame, marker in zip(cameras_to_remove, frames_with_reprojection_error,
- markers_with_reprojection_error):
- for camera in cameras:
- mask[camera, frame, marker, :] = False
- np.testing.assert_array_equal(result[mask], original_data[mask])
-
-
-if __name__ == "__main__":
- test_get_camera_frame_marker_lists_to_reproject()