Skip to content

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

TypeScript

shoveling together calibration whosits

jonmatthis94ef09c9061a
diff --git a/freemocap-ui/src/components/ui-components/Sidebar.tsx b/freemocap-ui/src/components/ui-components/Sidebar.tsx
index 8c74a8f9..d3a2e10c 100644
--- a/freemocap-ui/src/components/ui-components/Sidebar.tsx
+++ b/freemocap-ui/src/components/ui-components/Sidebar.tsx
@@ -35,7 +35,7 @@ const sidebarItems = [
// route: "/skeleton_detection"
// },
{
- id: "JS Detected Cameras",
+ id: "Config View",
route: "/config"
}
],
diff --git a/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/anipose_camera_calibrator.py b/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/anipose_camera_calibrator.py
index 1e1dd184..9bccac87 100644
--- a/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/anipose_camera_calibrator.py
+++ b/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/anipose_camera_calibrator.py
@@ -7,7 +7,7 @@ import numpy as np
import \
freemocap.core_processes.capture_volume_calibration.anipose_camera_calibration.anipose_stuff.anipose_charuco_board
from freemocap.core_processes.capture_volume_calibration.anipose_camera_calibration import (
- anipose_camera_group,
+ run_anipose_calibration_algorithm,
)
from freemocap.core_processes.capture_volume_calibration.charuco_stuff.charuco_board_definition import (
CharucoBoardDefinition,
diff --git a/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/get_anipose_calibration_object.py b/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/get_anipose_calibration_object.py
index 8e6d84f9..fa28c1b5 100644
--- a/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/get_anipose_calibration_object.py
+++ b/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/get_anipose_calibration_object.py
@@ -5,9 +5,9 @@ from pathlib import Path
from typing import Union
from freemocap.core_processes.capture_volume_calibration.anipose_camera_calibration import (
- anipose_camera_group,
+ run_anipose_calibration_algorithm,
)
-from freemocap.core_processes.capture_volume_calibration.anipose_camera_calibration.anipose_camera_group import \
+from freemocap.core_processes.capture_volume_calibration.anipose_camera_calibration.run_anipose_calibration_algorithm import \
AniposeCameraGroup
from freemocap.system.paths_and_filenames.path_getters import get_last_successful_calibration_toml_path
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/run_anipose_calibration_algorithm.py
similarity index 100%
rename from freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/anipose_camera_group.py
rename to freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/run_anipose_calibration_algorithm.py
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 245848d8..d5a10c4d 100644
--- a/freemocap/core_processes/capture_volume_calibration/triangulate_3d_data.py
+++ b/freemocap/core_processes/capture_volume_calibration/triangulate_3d_data.py
@@ -4,7 +4,7 @@ from pathlib import Path
import numpy as np
-from freemocap.core_processes.capture_volume_calibration.anipose_camera_calibration.anipose_camera_group import AniposeCameraGroup
+from freemocap.core_processes.capture_volume_calibration.anipose_camera_calibration.run_anipose_calibration_algorithm import AniposeCameraGroup
from freemocap.core_processes.capture_volume_calibration.save_3d_data_to_npy import (
save_3d_data_to_npy,
)
diff --git a/freemocap/pipelines/calibration_pipeline/calibration_aggregation_node.py b/freemocap/pipelines/calibration_pipeline/calibration_aggregation_node.py
index becc89d3..d2c4809a 100644
--- a/freemocap/pipelines/calibration_pipeline/calibration_aggregation_node.py
+++ b/freemocap/pipelines/calibration_pipeline/calibration_aggregation_node.py
@@ -1,5 +1,6 @@
import logging
import multiprocessing
+import threading
import time
from dataclasses import dataclass
from multiprocessing import Queue, Process
@@ -68,7 +69,8 @@ class CalibrationAggregationProcessNode(BaseAggregationNode):
shared_view_accumulator: SharedViewAccumulator = SharedViewAccumulator.create(
camera_ids=list(input_queues.keys()))
- calibrated = False
+ multi_camera_calibrator: MultiCameraCalibrator | None = None
+
try:
while not shutdown_event.is_set():
@@ -84,30 +86,29 @@ class CalibrationAggregationProcessNode(BaseAggregationNode):
f"Unexpected data type received from camera {camera_id}: {type(camera_node_output)}")
camera_node_incoming_data[camera_id] = camera_node_output
- if any([camera_node_output is None for camera_node_output in camera_node_incoming_data.values()]):
- logger.exception(f"Received None from camera nodes! got {camera_node_incoming_data}")
- raise ValueError(f"Received None from camera nodes! got {camera_node_incoming_data}")
-
frame_numbers = set(
[camera_node_output.frame_metadata.frame_number for camera_node_output in
camera_node_incoming_data.values()])
if len(frame_numbers) > 1:
- logger.exception(f"Frame numbers from camera nodes do not match! got {frame_numbers}")
raise ValueError(f"Frame numbers from camera nodes do not match! got {frame_numbers}")
multi_frame_number = frame_numbers.pop()
- if multi_frame_number % 10 == 0:
- # Accumulate shared views
- shared_view_accumulator.receive_camera_node_output(multi_frame_number=multi_frame_number,
- camera_node_output=camera_node_incoming_data)
- logger.trace(f"Shared view accumulator: {shared_view_accumulator.shared_view_count_by_camera}")
- if shared_view_accumulator.all_cameras_have_min_shared_views(10) and not calibrated:
- calibrated = True
- multi_camera_calibrator = MultiCameraCalibrator.initialize(
- shared_charuco_views=shared_view_accumulator.shared_target_views,
- calibrate_cameras=True)
-
- radius = 5
- frequency = 0.01
+
+ if multi_camera_calibrator is None:
+ if multi_frame_number % 10 == 0:
+ # Accumulate shared views
+ shared_view_accumulator.receive_camera_node_output(multi_frame_number=multi_frame_number,
+ camera_node_output=camera_node_incoming_data)
+ logger.trace(f"Shared view accumulator: {shared_view_accumulator.shared_view_count_by_camera}")
+ if shared_view_accumulator.all_cameras_have_min_shared_views(10) and multi_camera_calibrator is None:
+
+ multi_camera_calibrator = MultiCameraCalibrator.initialize(
+ shared_charuco_views=shared_view_accumulator.shared_target_views,
+ calibrate_cameras=True)
+ multi_camera_calibrator.calibrate()
+ else:
+ logger.info("Do triangulation and stuff with the calibration data")
+ radius = 1
+ frequency = 0.1
output = CalibrationPipelineOutputData(
camera_node_output=camera_node_incoming_data, # type: ignore
@@ -115,9 +116,9 @@ class CalibrationAggregationProcessNode(BaseAggregationNode):
multi_frame_number=multi_frame_number,
points3d={
camera_id: (
+ radius * np.cos(multi_frame_number * frequency) + camera_id,
camera_id,
- radius * np.cos(multi_frame_number * frequency) + camera_id * 5,
- radius * np.sin(multi_frame_number * frequency) + camera_id * 5
+ radius * np.sin(multi_frame_number * frequency) + camera_id
)
for camera_id in input_queues.keys()
},
diff --git a/freemocap/pipelines/calibration_pipeline/multi_camera_calibration/__init__.py b/freemocap/pipelines/calibration_pipeline/multi_camera_calibration/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/freemocap/pipelines/calibration_pipeline/multi_camera_calibration/anipose_calibration_algorithm.py b/freemocap/pipelines/calibration_pipeline/multi_camera_calibration/anipose_calibration_algorithm.py
new file mode 100644
index 00000000..ac5fbfb2
--- /dev/null
+++ b/freemocap/pipelines/calibration_pipeline/multi_camera_calibration/anipose_calibration_algorithm.py
@@ -0,0 +1,600 @@
+import logging
+from copy import deepcopy
+
+import cv2
+import numpy as np
+from numba import jit
+from pydantic import BaseModel
+from scipy import optimize
+from scipy.linalg import inv as inverse
+from scipy.sparse import dok_matrix
+from tqdm import trange
+
+from freemocap.pipelines.calibration_pipeline.multi_camera_calibration.calibration_numpy_types import PixelPoints2D, \
+ ObjectPoints3D, ExtrinsicsParameters, IntrinsicsParameters, ReprojectionError
+from freemocap.pipelines.calibration_pipeline.multi_camera_calibration.calibration_utilities import \
+ calculate_error_bounds, transform_points, construct_camera_extrinsics_matrix, \
+ get_rotation_and_translation_vector_from_extrinsics_matrix, get_error_dict
+from freemocap.pipelines.calibration_pipeline.multi_camera_calibrator import MultiCameraCalibrationInputData
+from freemocap.pipelines.calibration_pipeline.single_camera_calibration_estimate import SingleCameraCalibrationEstimate
+
+logger = logging.getLogger(__name__)
+
+
+
+
+
+class AniposeSingleCameraCalibrationHelper(BaseModel):
+ calibration_estimate: SingleCameraCalibrationEstimate
+
+
+ def get_optimizable_parameters(self) -> tuple[ExtrinsicsParameters, IntrinsicsParameters]:
+ """
+ Grabs the parameters that will be optimized and organizes them in a numpy array
+ """
+ extrinsics_parameters = np.zeros(6, dtype="float64")
+ extrinsics_parameters[0:3] = self.rotation_vector
+ extrinsics_parameters[3:6] = self.translation_vector
+
+ intrinsics_parameters = np.zeros(3, dtype="float64")
+ extrinsics_parameters[0] = self.focal_length
+ extrinsics_parameters[1] = self.distortion_coefficients[0]
+ extrinsics_parameters[2] = self.distortion_coefficients[1]
+ return extrinsics_parameters, intrinsics_parameters
+
+ def set_optimized_parameters(self,
+ extrinsics_parameters: ExtrinsicsParameters,
+ intrinsics_parameters: IntrinsicsParameters):
+
+ self.rotation_vector = extrinsics_parameters[0:3]
+ self.translation_vector = extrinsics_parameters[3:6]
+
+ self.focal_length = intrinsics_parameters[0]
+
+ distortion_coefficients = np.zeros(5)
+ distortion_coefficients[0] = intrinsics_parameters[1]
+ distortion_coefficients[1] = intrinsics_parameters[2]
+ self.distortion_coefficients = distortion_coefficients
+
+ def undistort_points(self, points2d: PixelPoints2D) -> PixelPoints2D:
+ """
+ Undistorts 2D points using the camera's intrinsic parameters and distortion coefficients.
+
+ This function adjusts the provided 2D points to account for lens distortion, returning
+ points that are mapped as they would appear in an ideal pinhole camera model. The process
+ ensures that the ray passing from the nodal point of the camera lens through the
+ undistorted 2D point intersects the corresponding 3D point in space accurately.
+
+ :param points2d: A NumPy array of shape (N, 2) or (N, 1, 2) representing the 2D points
+ to be undistorted. These are typically the distorted pixel coordinates
+ captured by the camera.
+ :return: A NumPy array of the same shape as `points2d` containing the undistorted 2D points.
+ """
+ shape = points2d.shape
+ points2d = points2d.reshape(-1, 1, 2)
+ undistorted_points2d = cv2.undistortPoints(points2d,
+ self.camera_matrix.astype("float64"),
+ self.distortion_coefficients.astype("float64"))
+ return undistorted_points2d.reshape(shape)
+
+ def project_3d_to_2d(self, points3d: ObjectPoints3D) -> PixelPoints2D:
+ if not points3d.shape[-1] == 3:
+ raise ValueError("points3d must have shape (N, 3)")
+ points3d = points3d.reshape(-1, 1, 3)
+ projected_2d_points, _ = cv2.projectPoints(
+ points3d,
+ self.rotation_vector,
+ self.translation_vector,
+ self.camera_matrix.astype("float64"),
+ self.distortion_coefficients.astype("float64"),
+ )
+ if projected_2d_points.shape != (points3d.shape[0], 1, 2):
+ raise ValueError(f"projected_2d_points has incorrect shape: {projected_2d_points.shape}")
+
+ return projected_2d_points
+
+ def single_camera_reprojection_error(self, points3d: ObjectPoints3D, points2d: PixelPoints2D) -> ReprojectionError:
+ projected_points3d = self.project_3d_to_2d(points3d)
+ projecting_3d_points_onto_2d_image_plane = projected_points3d.reshape(points2d.shape)
+ return points2d - projecting_3d_points_onto_2d_image_plane
+
+
+
+class AniposeMultiCameraCalibrator(BaseModel):
+
+ max_iterations: int = 10
+ starting_error_threshold: float = 15.0
+ ending_error_threshold: float = 1.0
+ max_number_function_evals: int = 200
+ function_tolerance: float = 1e-4
+ number_of_samples_per_iteration: int = 100
+ number_of_samples_full: int = 1000
+ error_threshold: float = 0.3
+
+ def run_iterative_bundle_adjustment(
+ self,
+ calibration_input_data: MultiCameraCalibrationInputData,
+
+ ) -> float:
+ """
+ Performs iterative bundle adjustment to refine camera parameters.
+ based on Anipose's iterative bundle adjustment method. (`bundle_adjust_iter`)
+ """
+ original_calibration_input_data = deepcopy(calibration_input_data)
+ original_points2d = original_calibration_input_data.pixel_points2d
+
+ # Validate input shapes
+ if not original_points2d.shape[0] == len(self.camera_calibration_estimates):
+ raise ValueError(
+ f"Invalid points shape, expected first dimension to be equal to "
+ f"number of cameras ({len(self.camera_calibration_estimates)}), but got shape {original_points2d.shape}"
+ )
+
+ # Initialize
+ error_list = []
+
+ # log original error before resampling
+ original_error = self.get_mean_reprojection_error(original_points2d, median=True)
+ logger.info(
+ f"Starting iterative bundle adjustment..."
+ f"\n\tNumber of cameras: {len(self.camera_calibration_estimates)}"
+ f"\n\tNumber of points: {original_points2d.shape[1]}"
+ f"\n\tOriginal error: {original_error:.2f}")
+
+ # Resample initial set of points to prioritize shared views
+ resampled_calibration_input_data = resample_points_based_on_shared_views(
+ original_calibration_input_data,
+ number_of_samples_to_return=self.number_of_samples_full
+ )
+ initial_error = self.get_mean_reprojection_error(resampled_calibration_input_data.pixel_points2d, median=True)
+
+ logger.debug("Initial error after resampling: ", initial_error)
+
+ # Calculate dynamic error thresholds for each iteration
+ error_thresholds = np.exp(
+ np.linspace(np.log(self.starting_error_threshold), np.log(self.ending_error_threshold),
+ num=self.max_iterations))
+
+ logger.debug(f"Error thresholds we will use for each bundle adjustment iteration: {error_thresholds}")
+
+ # Iterative bundle adjustment
+ dynamic_error_threshold = self.starting_error_threshold
+ for iteration in range(self.max_iterations):
+ resampled_calibration_input_data = resample_points_based_on_shared_views(
+ original_calibration_input_data, number_of_samples_to_return=self.number_of_samples_full
+ )
+
+ # Triangulate 3D points from resampled 2D points
+ triangulated_points3d = self.triangulate(resampled_calibration_input_data.pixel_points2d)
+
+ # Calculate reprojection errors
+ errors_full = self.calculate_reprojection_error(
+ triangulated_points3d, resampled_calibration_input_data.pixel_points2d, mean=False
+ )
+ normalized_errors = self.calculate_reprojection_error(
+ triangulated_points3d, resampled_calibration_input_data.pixel_points2d, mean=True
+ )
+
+ # Compute error statistics
+ error_dict = get_error_dict(errors_full)
+ 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
+ good_points_indicies = np.where(good_points_mask)[0]
+ # Update error list and check for convergence
+ current_error = np.median(normalized_errors)
+ error_list.append(current_error)
+ if current_error < self.error_threshold:
+ logger.debug(f"Converged after {iteration + 1} iterations")
+ break
+
+ 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_calibration_input_data.resample_indicies(list(good_points_indicies)),
+ loss="linear",
+ function_tolerance=self.function_tolerance,
+ maximum_number_function_evals=self.max_number_function_evals,
+ )
+
+ # Final adjustments and error calculation
+ final_calibration_input_data = resample_points_based_on_shared_views(
+ calibration_input_data=original_calibration_input_data,
+ number_of_samples_to_return=self.number_of_samples_full
+ )
+ self.final_adjustments(calibration_input_data=final_calibration_input_data,
+ error_threshold=dynamic_error_threshold
+ )
+
+ final_error = self.get_mean_reprojection_error(final_calibration_input_data.pixel_points2d, median=True)
+ print("Final error: ", final_error)
+
+ return float(final_error)
+
+ @jit(parallel=True, forceobj=True)
+ def calculate_reprojection_error(self,
+ points_3d: np.ndarray,
+ points_2d: np.ndarray,
+ mean=False):
+ """Given an Nx3 array of 3D points and an CxNx2 array of 2D points,
+ where N is the number of points and C is the number of cameras,
+ this returns an CxNx2 array of errors.
+ Optionally mean=True, this averages the errors and returns array of length N of errors"""
+ if not points_3d.shape[0] == points_2d.shape[1]:
+ raise ValueError(
+ f"Invalid points shape, first dim of 3D points should be equal to"
+ f" number of points in 2D points ({points_2d.shape[1]}), but shape is {points_3d.shape}"
+ )
+ if not points_2d.shape[0] == len(self.camera_calibration_estimates):
+ raise ValueError(
+ f"Invalid points shape, first dim of 2D points should be equal to"
+ f" number of cameras ({len(self.camera_calibration_estimates)}), but shape is {points_2d.shape}"
+ )
+ if not points_3d.shape[1] == 3:
+ raise ValueError(
+ f"Invalid points shape, second dim of 3D points should be 3, but shape is {points_3d.shape}")
+
+ if not points_2d.shape[2] == 2:
+ raise ValueError(
+ f"Invalid points shape, third dim of 2D points should be 2, but shape is {points_2d.shape}")
+
+ one_point = False
+ if len(points_3d.shape) == 1 and len(points_2d.shape) == 2:
+ points_3d = points_3d.reshape(1, 3)
+ points_2d = points_2d.reshape(-1, 1, 2)
+ one_point = True
+
+ n_cams, n_points, _ = points_2d.shape
+ if not points_3d.shape == (n_points, 3,):
+ raise ValueError("shapes of 2D and 3D points are not consistent: " "2D={}, 3D={}".format(points_2d.shape,
+ points_3d.shape))
+
+ errors = np.empty((n_cams, n_points, 2))
+
+ for camera_number, cam in enumerate(self.camera_calibration_estimates.values()):
+ errors[camera_number] = cam.single_camera_reprojection_error(points_3d, points_2d[camera_number])
+
+ if mean:
+ errors_norm = np.linalg.norm(errors, axis=2)
+ good = ~np.isnan(errors_norm)
+ errors_norm[~good] = 0
+ denom = np.sum(good, axis=0).astype("float64")
+ denom[denom < 1.5] = np.nan
+ errors = np.sum(errors_norm, axis=0) / denom
+
+ if one_point:
+ if mean:
+ errors = float(errors[0])
+ else:
+ errors = errors.reshape(-1, 2)
+
+ return errors
+
+ def project_3d_to_2d(self, points3d: np.ndarray):
+ """Given an Nx3 array of points, this returns an CxNx2 array of 2D points,
+ where C is the number of cameras"""
+ points3d = points3d.reshape(-1, 1, 3)
+ n_points = points3d.shape[0]
+ n_cams = len(self.camera_calibration_estimates)
+
+ projected_points2d = np.empty((n_cams, n_points, 2), dtype="float64")
+ for camera_number, camera in enumerate(self.camera_calibration_estimates.values()):
+ projected_points2d[camera_number] = camera.project_3d_to_2d(points3d).reshape(n_points, 2)
+
+ # check shape
+ assert projected_points2d.shape == (n_cams, n_points,
+ 2), f"Invalid projected_points2d shape, should be {n_cams, n_points, 2}, but shape is {projected_points2d.shape}"
+
+ return projected_points2d
+
+ def triangulate(self, points2d, undistort=True, progress=False):
+ """Given an CxNx2 array, this returns an Nx3 array of points,
+ where N is the number of points and C is the number of cameras"""
+
+ assert points2d.shape[0] == len(
+ self.camera_calibration_estimates
+ ), "Invalid points shape, first dim should be equal to" " number of cameras ({}), but shape is {}".format(
+ len(self.camera_calibration_estimates), points2d.shape
+ )
+
+ one_point = False
+ if len(points2d.shape) == 2:
+ points2d = points2d.reshape(-1, 1, 2)
+ one_point = True
+
+ if undistort:
+ new_points = np.empty(points2d.shape)
+ for camera_number, camera in enumerate(self.camera_calibration_estimates.values()):
+ # must copy in order to satisfy opencv underneath
+ sub = np.copy(points2d[camera_number])
+ new_points[camera_number] = camera.undistort_points(sub)
+ points2d = new_points
+
+ n_cams, n_points, _ = points2d.shape
+
+ triangulated_points3d = np.empty((n_points, 3))
+ triangulated_points3d[:] = np.nan
+
+ camera_matricies = np.array([camera.extrinsics_matrix for camera in self.camera_calibration_estimates.values()])
+
+ if progress:
+ points_iterator = trange(n_points, ncols=70)
+ else:
+ points_iterator = range(n_points)
+
+ for point_index in points_iterator:
+ point_xy = points2d[:, point_index, :]
+ point_xy_no_nans = ~np.isnan(point_xy[:, 0])
+ if np.sum(point_xy_no_nans) >= 2:
+ triangulated_points3d[point_index] = anipose_triangulate_simple(point_xy[point_xy_no_nans],
+ camera_matricies[point_xy_no_nans])
+
+ if one_point:
+ triangulated_points3d = triangulated_points3d[0]
+
+ return triangulated_points3d
+
+ def final_adjustments(self,
+ calibration_input_data: MultiCameraCalibrationInputData,
+ error_threshold: float) -> None:
+ """Perform final adjustments and bundle adjustment with a potentially updated threshold. """
+ triangulated_points3d = self.triangulate(calibration_input_data.pixel_points2d)
+ errors_full = self.calculate_reprojection_error(
+ triangulated_points3d, calibration_input_data.pixel_points2d, mean=False
+ )
+ normalized_errors = self.calculate_reprojection_error(
+ triangulated_points3d, calibration_input_data.pixel_points2d, mean=True
+ )
+ error_dict = get_error_dict(errors_full)
+
+ logger.debug("Error dictionary after final adjustments", error_dict)
+
+ max_error, min_error = calculate_error_bounds(error_dict)
+ dynamic_error_threshold = max(max(max_error, error_threshold), min_error)
+ good_points_mask = normalized_errors < dynamic_error_threshold
+ good_points_indicies = np.where(good_points_mask)[0]
+ self.bundle_adjust_points2d(
+ calibration_input_data.resample_indicies(list(good_points_indicies)),
+ loss="linear",
+ function_tolerance=self.function_tolerance,
+ maximum_number_function_evals=self.max_number_function_evals,
+ )
+
+ def bundle_adjust_points2d(
+ self,
+ calibration_input_data: MultiCameraCalibrationInputData,
+ 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,
+
+ ) -> float:
+ """Perform bundle adjustment to fine-tune camera parameters"""
+ points2d = calibration_input_data.pixel_points2d
+ # 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.camera_calibration_estimates):
+ raise ValueError(
+ f"Invalid points shape, expected first dimension to be equal to "
+ f"number of cameras ({len(self.camera_calibration_estimates)}), but got shape {points2d.shape}"
+ )
+
+ # Initialize parameters for bundle adjustment
+ initial_params, num_camera_params = self._initalize_bundle_adjust_parameters(calibration_input_data)
+
+ # Override initial parameters if start_params are provided
+ if start_params is not None:
+ initial_params = start_params
+ num_camera_params = len(self.camera_calibration_estimates[0].get_optimizable_parameters())
+
+ # Define error function and sparsity pattern for optimization
+
+ jac_sparsity = self._calculate_bundle_adjustment_jacobian_sparsity(calibration_input_data, num_camera_params)
+
+ # 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=function_tolerance,
+ method="trf",
+ tr_solver="lsmr",
+ verbose=2,
+ max_nfev=maximum_number_function_evals,
+ args=(points2d, num_camera_params),
+ )
+
+ # Update camera parameters with optimized results
+ optimized_params = optimization_result.x
+ for i, camera in enumerate(self.camera_calibration_estimates.values()):
+ start_idx = i * num_camera_params
+ end_idx = (i + 1) * num_camera_params
+ camera.set_optimized_parameters(optimized_params[start_idx:end_idx])
+
+ # 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 _bundle_adjust_error_function(self,
+ optimizable_parameters: np.ndarray,
+ calibration_input_data: MultiCameraCalibrationInputData,
+ number_of_camera_parameters: int,
+ ):
+ """Error function for bundle adjustment"""
+ points2d = calibration_input_data.pixel_points2d
+ good = ~np.isnan(points2d)
+ n_cams = len(self.camera_calibration_estimates)
+
+ for i, cam in enumerate(self.camera_calibration_estimates.values()):
+ cam = self.camera_calibration_estimates[i]
+ a = i * number_of_camera_parameters
+ b = (i + 1) * number_of_camera_parameters
+ cam.set_optimized_parameters(optimizable_parameters[a:b])
+
+ n_cams = len(self.camera_calibration_estimates)
+ sub = number_of_camera_parameters * n_cams
+ n3d = points2d.shape[1] * 3
+ p3ds_test = optimizable_parameters[sub: sub + n3d].reshape(-1, 3)
+ errors = self.calculate_reprojection_error(p3ds_test, points2d)
+ errors_reproj = errors[good]
+
+ ids = remap_ids(calibration_input_data.ids)
+ object_points3d = calibration_input_data.object_points3d
+ min_scale = np.min(object_points3d[object_points3d > 0])
+ n_boards = int(np.max(ids)) + 1
+ a = sub + n3d
+ rotation_vectors = optimizable_parameters[a: a + n_boards * 3].reshape(-1, 3)
+ translation_vectors = optimizable_parameters[a + n_boards * 3: a + n_boards * 6].reshape(-1, 3)
+ expected = transform_points(object_points3d, rotation_vectors[ids], translation_vectors[ids])
+ errors_obj = 2 * (p3ds_test - expected).ravel() / min_scale
+
+ return np.hstack([errors_reproj, errors_obj])
+
+ def _calculate_bundle_adjustment_jacobian_sparsity(self,
+ calibration_input_data: MultiCameraCalibrationInputData,
+ number_of_camear_parameters: int) -> 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"""
+ points2d = calibration_input_data.pixel_points2d
+ point_indices = np.zeros(points2d.shape, dtype="int32")
+ cam_indices = np.zeros(points2d.shape, dtype="int32")
+
+ for i in range(points2d.shape[1]):
+ point_indices[:, i] = i
+
+ for j in range(points2d.shape[0]):
+ cam_indices[j] = j
+
+ good = ~np.isnan(points2d)
+
+ ids = remap_ids(calibration_input_data.ids)
+ n_boards = int(np.max(ids)) + 1
+ total_board_params = n_boards * (3 + 3) # rotation_vectors + translation_vectors
+
+ n_cams = points2d.shape[0]
+ n_points = points2d.shape[1]
+ 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)
+
+ n_errors = n_good_values + n_points * 3
+
+ A_sparse = dok_matrix((n_errors, n_params), dtype="int16")
+
+ cam_indices_good = cam_indices[good]
+ point_indices_good = point_indices[good]
+
+ # -- reprojection error --
+ ix = np.arange(n_good_values)
+
+ ## update camera params based on point error
+ 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 * number_of_camear_parameters + point_indices_good * 3 + i] = 1
+
+ # -- match for the object points--
+
+ point_ix = np.arange(n_points)
+
+ ## update all the camera parameters
+ # A_sparse[n_good_values:n_good_values+n_points*3,
+ # 0:n_cams*n_cam_params] = 1
+
+ ## update board rotation and translation based on error from expected
+ for i in range(3):
+ for j in range(3):
+ A_sparse[
+ n_good_values + point_ix * 3 + i,
+ total_params_reproj + ids * 3 + j,
+ ] = 1
+ A_sparse[
+ n_good_values + point_ix * 3 + i,
+ total_params_reproj + n_boards * 3 + ids * 3 + j,
+ ] = 1
+
+ ## update point position based on error from expected
+ for i in range(3):
+ A_sparse[
+ n_good_values + point_ix * 3 + i,
+ n_cams * number_of_camear_parameters + point_ix * 3 + i,
+ ] = 1
+
+ return A_sparse
+
+ def _initalize_bundle_adjust_parameters(self, calibration_input_data:MultiCameraCalibrationInputData):
+ """Given an CxNx2 array of 2D points,
+ where N is the number of points and C is the number of cameras,
+ initializes the parameters for bundle adjustment"""
+
+ cam_params = np.hstack([cam.get_optimizable_parameters() for cam in self.camera_calibration_estimates.values()])
+ n_cam_params = len(cam_params) // len(self.camera_calibration_estimates)
+
+ total_cam_params = len(cam_params)
+
+ n_cams, n_points, _ = calibration_input_data.pixel_points2d.shape
+ assert n_cams == len(self.camera_calibration_estimates), (
+ "number of cameras in CameraGroup does not " "match number of cameras in 2D points given"
+ )
+
+ points3d = self.triangulate(calibration_input_data.pixel_points2d)
+
+ ids = remap_ids(calibration_input_data.ids)
+ n_boards = int(np.max(ids[~np.isnan(ids)])) + 1
+ total_board_params = n_boards * (3 + 3) # rotation_vectors + translation_vectors
+
+ # initialize to 0
+ rotation_vectors = np.zeros((n_boards, 3), dtype="float64")
+ translation_vectors = np.zeros((n_boards, 3), dtype="float64")
+
+
+ rotation_vectors_all = calibration_input_data.rotation_vectors
+ translation_vectors_all = calibration_input_data.translation_vectors
+ for board_num in range(n_boards):
+ point_id = np.where(ids == board_num)[0][0]
+ cam_ids_possible = np.where(~np.isnan(calibration_input_data.pixel_points2d[:, point_id, 0]))[0]
+ cam_id = np.random.choice(cam_ids_possible)
+ camera_extrinsics_matrix = self.camera_calibration_estimates[cam_id].extrinsics_matrix
+ M_board_cam = construct_camera_extrinsics_matrix(rotation_vectors_all[cam_id, point_id],
+ translation_vectors_all[cam_id, point_id])
+ M_board = np.matmul(inverse(camera_extrinsics_matrix), M_board_cam)
+ rotation_vector, translation_vector = get_rotation_and_translation_vector_from_extrinsics_matrix(
+ M_board)
+ rotation_vectors[board_num] = rotation_vector
+ translation_vectors[board_num] = translation_vector
+
+ x0 = np.zeros(total_cam_params + points3d.size + total_board_params)
+ x0[:total_cam_params] = cam_params
+ x0[total_cam_params: total_cam_params + points3d.size] = points3d.ravel()
+
+ start_board = total_cam_params + points3d.size
+ x0[start_board: start_board + n_boards * 3] = rotation_vectors.ravel()
+ x0[start_board + n_boards * 3: start_board + n_boards * 6] = translation_vectors.ravel()
+
+ return x0, n_cam_params
+
+ 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)
diff --git a/freemocap/pipelines/calibration_pipeline/multi_camera_calibration/calibration_numpy_types.py b/freemocap/pipelines/calibration_pipeline/multi_camera_calibration/calibration_numpy_types.py
new file mode 100644
index 00000000..b2803b7e
--- /dev/null
+++ b/freemocap/pipelines/calibration_pipeline/multi_camera_calibration/calibration_numpy_types.py
@@ -0,0 +1,24 @@
+import numpy as np
+from numpydantic import NDArray, Shape
+
+RotationVector = NDArray[Shape["3"], np.float64]
+TranslationVector = NDArray[Shape["3"], np.float64]
+PixelPoints2D = NDArray[Shape["* n_points, 2 pixelx_pixely"], np.float64]
+ReprojectionError = NDArray[Shape["* n_points"], np.float64]
+CameraMatrix = NDArray[Shape["3 rows, 3 columns"], np.float64]
+CameraDistortionCoefficients = NDArray[Shape["4-14 k1_k2_p1_p2_k3_k4_k5_k6"], np.float64] #Can be 4,5,8,12, or 14 elements
+
+RotationVectorsByCamera = NDArray[Shape["* n_cams, 3"], np.float64]
+TranslationVectorsByCamera = NDArray[Shape["* n_cams, 3"], np.float64]
+PixelPoints2DByCamera = NDArray[Shape["* n_cams, * n_points, 2 pixelx_pixely"], np.float64]
+CameraMatrixByCamera = NDArray[Shape["* n_cams, 3 rows, 3 columns"], np.float64]
+
+TransformationMatrix = NDArray[Shape["4 rows, 4 columns"], np.float64] # 4x4 matrix that transforms points from the camera coordinate system to the world coordinate system. 3x3 in the upper left is the rotation matrix, and the rightmost column is the translation vector. The bottom row is [0, 0, 0, 1] (where the 1 is the homogeneous coordinate, which makes the matrix invertible and provides the scale factor for the translation vector to put it in spatial coordinates
+ObjectPoints3D = NDArray[Shape["* n_points, 3 xyz"], np.float64]
+PointIds = NDArray[Shape["* n_points"], np.int64]
+
+ExtrinsicsParameters = NDArray[Shape["6 translation_rotation"], np.float64] # 3 for rotation, 3 for translation
+IntrinsicsParameters = NDArray[Shape["*, ..."], np.float64] # focal length, variable number of distortion coefficients (2, 4, 5, 8, 12, or 14)
+
+ExtrinsicsParametersByCamera = NDArray[Shape["* n_cams, 6 translation_rotation"], np.float64]
+IntrinsicsParametersByCamera = NDArray[Shape["* n_cams, * focal_length_distortion"], np.float64]
diff --git a/freemocap/pipelines/calibration_pipeline/multi_camera_calibration/calibration_utilities.py b/freemocap/pipelines/calibration_pipeline/multi_camera_calibration/calibration_utilities.py
new file mode 100644
index 00000000..753e291c
--- /dev/null
+++ b/freemocap/pipelines/calibration_pipeline/multi_camera_calibration/calibration_utilities.py
@@ -0,0 +1,84 @@
+import cv2
+import numpy as np
+
+from freemocap.pipelines.calibration_pipeline.multi_camera_calibration.calibration_numpy_types import RotationVector, \
+ TranslationVector, ObjectPoints3D
+
+
+def calculate_error_bounds(error_dict: dict[tuple[int, int], tuple[int, np.ndarray]]) -> tuple[float, float]:
+ """Calculate the maximum and minimum error bounds from an error dictionary.
+
+ Args:
+ error_dict (Dict[Tuple[int, int], Tuple[int, np.ndarray]]): Dictionary containing error statistics
+ for camera pairs, where the key is a tuple of camera indices and the value is a tuple of
+ (number of observations, percentiles).
+
+ Returns:
+ Tuple[float, float]: A tuple of (max_error, min_error) based on the percentiles of error means.
+ """
+ max_error = 0.0
+ min_error = float('inf')
+ for _, (_, percents) in error_dict.items():
+ max_error = max(percents[-1], max_error)
+ min_error = min(percents[0], min_error)
+ return max_error, min_error
+
+
+def transform_points(points: ObjectPoints3D,
+ rotation_vector: RotationVector,
+ translation_vector: TranslationVector):
+ """Rotate points by given rotation vectors and translate.
+ Rodrigues' rotation formula is used.
+ """
+ theta = np.linalg.norm(rotation_vector, axis=1)[:, np.newaxis]
+ with np.errstate(invalid="ignore"):
+ v = rotation_vector / theta
+ v = np.nan_to_num(v)
+ dot = np.sum(points * v, axis=1)[:, np.newaxis]
+ cos_theta = np.cos(theta)
+ sin_theta = np.sin(theta)
+
+ rotated = cos_theta * points + sin_theta * np.cross(v, points) + dot * (1 - cos_theta) * v
+
+ return rotated + translation_vector
+
+
+def construct_camera_extrinsics_matrix(rotation_vector: RotationVector,
+ translation_vector: TranslationVector) -> np.ndarray:
+ if rotation_vector.shape != (3,) or translation_vector.shape != (3,):
+ raise ValueError("Rotation vector and translation vector must have shape (3,)")
+ extrinsics_matrix = np.zeros((4, 4))
+ rotmat, _ = cv2.Rodrigues(rotation_vector)
+ extrinsics_matrix[:3, :3] = rotmat
+ extrinsics_matrix[:3, 3] = translation_vector.flatten()
+ extrinsics_matrix[3, 3] = 1
+ return extrinsics_matrix
+
+
+def get_rotation_and_translation_vector_from_extrinsics_matrix(extrinsics_matrix: np.ndarray) -> tuple[
+ np.ndarray, np.ndarray]:
+ if extrinsics_matrix.shape != (4, 4):
+ raise ValueError("Extrinsics matrix must be a 4x4 matrix")
+ rotation_vector = cv2.Rodrigues(extrinsics_matrix[:3, :3])[0].flatten()
+ translation_vector = extrinsics_matrix[:3, 3].flatten()
+ return rotation_vector, translation_vector
+
+
+def get_error_dict(errors_full, min_points=10):
+ n_cams = errors_full.shape[0]
+ errors_norm = np.linalg.norm(errors_full, axis=2)
+
+ good = ~np.isnan(errors_full[:, :, 0])
+
+ error_dict = dict()
+
+ for i in range(n_cams):
+ for j in range(i + 1, n_cams):
+ subset = good[i] & good[j]
+ err_subset = errors_norm[:, subset][[i, j]]
+ err_subset_mean = np.mean(err_subset, axis=0)
+ if np.sum(subset) > min_points:
+ percents = np.percentile(err_subset_mean, [15, 75])
+ # percents = np.percentile(err_subset, [25, 75])
+ error_dict[(i, j)] = (err_subset.shape[1], percents)
+ return error_dict
diff --git a/freemocap/pipelines/calibration_pipeline/multi_camera_calibration/triangulation_method.py b/freemocap/pipelines/calibration_pipeline/multi_camera_calibration/triangulation_method.py
new file mode 100644
index 00000000..5c58f298
--- /dev/null
+++ b/freemocap/pipelines/calibration_pipeline/multi_camera_calibration/triangulation_method.py
@@ -0,0 +1,32 @@
+import numpy as np
+from numba import jit
+
+
+@jit(nopython=True, parallel=False)
+def triangulate_simple(points2d: np.ndarray,
+ camera_matrices: np.ndarray) -> np.ndarray:
+ """
+ Triangulate 3D point from 2D points and camera matrices using the Direct Linear Transform (DLT) method.
+
+ :param points2d: A 2D array of shape (N, 2), where N is the number of cameras. Each row represents the
+ 2D coordinates (x, y) of the point in the image plane of each camera.
+ :param camera_matrices: A 3D array of shape (N, 3, 4), where each element is a 3x4 camera projection matrix.
+ :return: A 1D array of shape (3,) representing the triangulated 3D point.
+ """
+
+ number_of_cameras = camera_matrices.shape[0]
+ A = np.zeros((number_of_cameras * 2, 4))
+
+ for i in range(number_of_cameras):
+ x, y = points2d[i]
+ mat = camera_matrices[i]
+ A[i * 2] = x * mat[2] - mat[0]
+ A[i * 2 + 1] = y * mat[2] - mat[1]
+
+ _, _, vh = np.linalg.svd(A,
+ full_matrices=False) # CHECK - Ai said this could be set to False since " we only need the right singular vectors, which is more efficient." but we should check on this bc I don't know what that means lol
+
+ points3d_xyzh = vh[-1]
+ points3d_xyzh /= points3d_xyzh[3] # Normalize the last element to 1
+
+ return points3d_xyzh[:3]
diff --git a/freemocap/pipelines/calibration_pipeline/multi_camera_calibration_estimate.py b/freemocap/pipelines/calibration_pipeline/multi_camera_calibration_estimate.py
deleted file mode 100644
index 06275742..00000000
--- a/freemocap/pipelines/calibration_pipeline/multi_camera_calibration_estimate.py
+++ /dev/null
@@ -1,11 +0,0 @@
-from pydantic import BaseModel
-from skellycam import CameraId
-
-from freemocap.pipelines.calibration_pipeline.positional_6dof import Positional6DOF
-from freemocap.pipelines.calibration_pipeline.single_camera_calibration_estimate import SingleCameraCalibrationEstimate
-
-
-class MultiCameraCalibrationEstimate(BaseModel):
- principal_camera_id: CameraId
- principal_camera_6dof: Positional6DOF
- camera_estimates: dict[CameraId, SingleCameraCalibrationEstimate]
diff --git a/freemocap/pipelines/calibration_pipeline/multi_camera_calibrator.py b/freemocap/pipelines/calibration_pipeline/multi_camera_calibrator.py
index 29b7f1ff..42fd8223 100644
--- a/freemocap/pipelines/calibration_pipeline/multi_camera_calibrator.py
+++ b/freemocap/pipelines/calibration_pipeline/multi_camera_calibrator.py
@@ -1,15 +1,24 @@
from pydantic import BaseModel
+from skellycam import CameraId
-from freemocap.pipelines.calibration_pipeline.calibration_camera_node_output_data import CalibrationCameraNodeOutputData
-from freemocap.pipelines.calibration_pipeline.multi_camera_calibration_estimate import MultiCameraCalibrationEstimate
-from freemocap.pipelines.calibration_pipeline.positional_6dof import Positional6DOF
-from freemocap.pipelines.calibration_pipeline.shared_view_accumulator import SharedViewAccumulator, TargetViewByCamera
+from freemocap.pipelines.calibration_pipeline.multi_camera_calibration.calibration_numpy_types import \
+ PixelPoints2DByCamera, ObjectPoints3D, PointIds, RotationVectorsByCamera, TranslationVectorsByCamera
+from freemocap.pipelines.calibration_pipeline.shared_view_accumulator import TargetViewByCamera
from freemocap.pipelines.calibration_pipeline.single_camera_calibrator import SingleCameraCalibrator
+class MultiCameraCalibrationInputData(BaseModel):
+ pixel_points2d: PixelPoints2DByCamera
+ object_points3d: ObjectPoints3D
+ points_ids: PointIds
+ rotation_vectors: RotationVectorsByCamera
+ translation_vectors: TranslationVectorsByCamera
+
+import logging
+logger = logging.getLogger(__name__)
-from skellycam import CameraId
class MultiCameraCalibrator(BaseModel):
- calibration_estimate: MultiCameraCalibrationEstimate
- charuco_views_by_cameras: dict[CameraId, list[CalibrationCameraNodeOutputData]]
+ principal_camera_id: CameraId
+ camera_id_to_index: dict[CameraId, int]
+ single_camera_calibrators: dict[CameraId, SingleCameraCalibrator]
@classmethod
def initialize(cls,
@@ -28,15 +37,21 @@ class MultiCameraCalibrator(BaseModel):
# Find the principal camera (usually Camera0, but this finds the lowest indexed camera)
principal_camera_id = min(key for key in charuco_view_by_camera.keys())
- principal_camera_6dof = Positional6DOF(translation=[0., 0., 0.],
- rotation=[0., 0., 0.])
- camera_estimates = {}
+ single_camera_calibrators = {}
for camera_id, camera_node_outputs in charuco_view_by_camera.items():
- camera_estimates[camera_id] = SingleCameraCalibrator.from_camera_node_outputs(camera_node_outputs=camera_node_outputs,
- calibrate_camera=calibrate_cameras).current_estimate
+ single_camera_calibrators[camera_id] = SingleCameraCalibrator.from_camera_node_outputs(
+ camera_node_outputs=camera_node_outputs,
+ calibrate_camera=calibrate_cameras)
+
+
+ return cls(principal_camera_id=principal_camera_id,
+ single_camera_calibrators=single_camera_calibrators,
+ camera_id_to_index={camera_id: index for index, camera_id in enumerate(single_camera_calibrators.keys())}
+ )
- return cls(calibration_estimate=MultiCameraCalibrationEstimate(principal_camera_id=principal_camera_id,
- principal_camera_6dof=principal_camera_6dof,
- camera_estimates=camera_estimates),
- charuco_views_by_cameras=charuco_view_by_camera)
+ def calibrate(self):
+ for single_camera_calibrator in self.single_camera_calibrators.values():
+ single_camera_calibrator.update_calibration_estimate()
+ logger.trace(f"Calibrated camera {single_camera_calibrator.camera_id}:\n{single_camera_calibrator.current_estimate.model_dump_json(indent=2)}")
+ f=9
\ No newline at end of file
diff --git a/freemocap/pipelines/calibration_pipeline/positional_6dof.py b/freemocap/pipelines/calibration_pipeline/positional_6dof.py
index 0cde934a..4a1e0401 100644
--- a/freemocap/pipelines/calibration_pipeline/positional_6dof.py
+++ b/freemocap/pipelines/calibration_pipeline/positional_6dof.py
@@ -1,7 +1,26 @@
+import numpy as np
from numpydantic import NDArray, Shape
from pydantic import BaseModel
+from freemocap.pipelines.calibration_pipeline.multi_camera_calibration.calibration_numpy_types import \
+ TransformationMatrix
+import cv2
-class Positional6DOF(BaseModel):
- translation: NDArray[Shape["3 xyz"], float]
- rotation: NDArray[Shape["3 xyz"], float]
+class Positional6DoF(BaseModel):
+ translation: NDArray[Shape["3 xyz"], float] = np.zeros((3))
+ rotation: NDArray[Shape["3 xyz"], float] = np.zeros((3))
+
+ @property
+ def transformation_matrix(self) -> TransformationMatrix:
+ """
+ Returns the transformation matrix for this 6DoF pose in the form of a 4x4 matrix (homogeneous coordinates)
+ the left upper 3x3 matrix is the rotation matrix, and the rightmost column is the translation vector.
+ the bottom row is [0, 0, 0, 1] (where the 1 is the homogeneous coordinate, which makes the matrix invertible and provides the scale factor for the translation vector to put it in spatial coordinates)
+ """
+
+ transformation_matrix = np.zeros((4, 4))
+
+ transformation_matrix[:3, :3] = cv2.Rodrigues(self.rotation_vector)
+ transformation_matrix[:3, 3] = self.translation_vector.flatten()
+ transformation_matrix[3, 3] = 1
+ return transformation_matrix
diff --git a/freemocap/pipelines/calibration_pipeline/single_camera_calibration_estimate.py b/freemocap/pipelines/calibration_pipeline/single_camera_calibration_estimate.py
index b9b3b361..11567fb2 100644
--- a/freemocap/pipelines/calibration_pipeline/single_camera_calibration_estimate.py
+++ b/freemocap/pipelines/calibration_pipeline/single_camera_calibration_estimate.py
@@ -2,10 +2,34 @@ import numpy as np
from numpydantic import NDArray, Shape
from pydantic import BaseModel
+from pydantic import Field
+
+from freemocap.pipelines.calibration_pipeline.multi_camera_calibration.calibration_numpy_types import \
+ TransformationMatrix
+from freemocap.pipelines.calibration_pipeline.positional_6dof import Positional6DoF
+from skellycam import CameraId
class SingleCameraCalibrationEstimate(BaseModel):
+ camera_id: CameraId
+
camera_matrix: NDArray[Shape["3, 3"], np.float64]
distortion_coefficients: NDArray[Shape["5"], np.float64]
- mean_reprojection_errors: list[float]
- camera_calibration_residuals: list[float]
- reprojection_error_by_view: list[float]
+
+ positional_6dof: Positional6DoF = Field(default_factory=Positional6DoF)
+
+
+ @property
+ def focal_length(self):
+ fx = self.camera_matrix[0, 0]
+ fy = self.camera_matrix[1, 1]
+ return (fx + fy) / 2.0
+
+ @focal_length.setter
+ def focal_length(self, fx: float, fy: float = None):
+ if fy is None:
+ fy = fx
+ self.camera_matrix[0, 0] = fx
+ self.camera_matrix[1, 1] = fy
+
+ def extrinsic_matrix(self) -> TransformationMatrix:
+ return self.positional_6dof.transformation_matrix
\ No newline at end of file
diff --git a/freemocap/pipelines/calibration_pipeline/single_camera_calibrator.py b/freemocap/pipelines/calibration_pipeline/single_camera_calibrator.py
index f9d3fc14..96a8b689 100644
--- a/freemocap/pipelines/calibration_pipeline/single_camera_calibrator.py
+++ b/freemocap/pipelines/calibration_pipeline/single_camera_calibrator.py
@@ -28,22 +28,13 @@ class SingleCameraCalibrator(BaseModel):
SingleCameraCalibrator class for estimating camera calibration parameters.
cv2.calibrateCamera docs: https://docs.opencv.org/4.10.0/d9/d0c/group__calib3d.html#ga687a1ab946686f0d85ae0363b5af1d7b
-
- Attributes:
- camera_matrix (np.ndarray[3, 3]): 3x3 matrix with rows: [focal_length_x, 0, center_point_x],
- [0, focal_length_y, center_point_y], [0, 0, 1]
- distortion_coefficients (np.ndarray[..., 1]): Distortion coefficients to include, e.g. [k1, k2, p1, p2, k3].
- Can be 4, 5, 8, 12, or 14 elements.
- object_points_views (list[np.ndarray[..., 3]]): List of 3D points in the world (or object) coordinate space.
- image_points_views (list[np.ndarray[..., 2]]): List of 2D points in the image coordinate space (i.e. pixel coordinates).
- rotation_vectors (list[np.ndarray[..., 3]]): List of rotation vectors, containing the rotation of the camera
- relative to the world coordinate space for each view.
- translation_vectors (list[np.ndarray[..., 3]]): List of translation vectors, containing the translation of the camera
- relative to the world coordinate space for each view.
- error (SingleCameraCalibrationError | None): Calibration error details.
"""
camera_id: CameraId
image_size: tuple[int, ...]
+ object_points_views: list[NDArray[Shape["*, 3"], np.float32]]
+ image_points_views: list[NDArray[Shape["*, 2"], np.float32]]
+ rotation_vectors: list[NDArray[Shape["*, 3"], np.float32]]
+ translation_vectors: list[NDArray[Shape["*, 3"], np.float32]]
charuco_corner_ids: list[int]
charuco_corners_in_object_coordinates: NDArray[Shape["*, 3"], np.float32]
@@ -55,10 +46,6 @@ class SingleCameraCalibrator(BaseModel):
camera_matrix: NDArray[Shape["3 rows, 3 columns"], np.float64]
charuco_observations: CharucoObservations = Field(default_factory=CharucoObservations)
- object_points_views: list[NDArray[Shape["*, 3"], np.float32]]
- image_points_views: list[NDArray[Shape["*, 2"], np.float32]]
- rotation_vectors: list[NDArray[Shape["*, 3"], np.float32]]
- translation_vectors: list[NDArray[Shape["*, 3"], np.float32]]
reprojection_error_by_view: list[float]
mean_reprojection_errors: list[float]
camera_calibration_residuals: list[float]
@@ -70,17 +57,9 @@ class SingleCameraCalibrator(BaseModel):
@property
def current_estimate(self) -> SingleCameraCalibrationEstimate:
return SingleCameraCalibrationEstimate(
+ camera_id=self.camera_id,
camera_matrix=self.camera_matrix,
distortion_coefficients=self.distortion_coefficients,
- mean_reprojection_errors=self.mean_reprojection_errors,
- camera_calibration_residuals=self.camera_calibration_residuals,
- reprojection_error_by_view=self.reprojection_error_by_view,
- # charuco_observations=self.charuco_observations,
- # object_points_views=self.object_points_views,
- # image_points_views=self.image_points_views,
- # rotation_vectors=self.rotation_vectors,
- # translation_vectors=self.translation_vectors,
- #
)
@classmethod
@@ -164,6 +143,7 @@ class SingleCameraCalibrator(BaseModel):
f"#Current views: {len(self.object_points_views)}, "
f"#Charuco corners: {len(self.charuco_corner_ids)}")
+ # https://docs.opencv.org/4.10.0/d9/d0c/group__calib3d.html#ga687a1ab946686f0d85ae0363b5af1d7b
(residual,
self.camera_matrix,
self.distortion_coefficients,
@@ -182,10 +162,14 @@ class SingleCameraCalibrator(BaseModel):
f"camera_matrix: {self.camera_matrix}",
f"distortion_coefficients: {self.distortion_coefficients}",
)
+ self.rotation_vectors = [np.squeeze(rotation_vector) for rotation_vector in self.rotation_vectors]
+ self.translation_vectors = [np.squeeze(translation_vector) for translation_vector in self.translation_vectors]
self.camera_calibration_residuals.append(residual)
self._update_reprojection_error()
self._drop_suboptimal_views()
+
+
def get_board_pose(self, object_points: np.ndarray[..., 3], image_points: np.ndarray[..., 2]) -> tuple[
np.ndarray[..., 3], np.ndarray[..., 3]]:
if len(object_points) < 6:
@@ -241,19 +225,22 @@ class SingleCameraCalibrator(BaseModel):
self.charuco_observations = [self.charuco_observations[i] for i in sorted_indices[:number_of_views_to_keep]]
def _update_reprojection_error(self):
+ # https://docs.opencv.org/4.10.0/d9/d0c/group__calib3d.html#ga1019495a2c8d1743ed5cc23fa0daff8c
if len(self.image_points_views) != len(self.object_points_views):
raise ValueError("The number of image and object points must be the same")
if len(self.image_points_views) == 0:
raise ValueError("No image points provided")
error = None
- jacobian = None
+ jacobian = None #2Nx(10+) jacobian
self.reprojection_error_by_view = []
for view_index in range(len(self.image_points_views)):
- projected_image_points, jacobian = cv2.projectPoints(self.object_points_views[view_index],
- self.rotation_vectors[view_index],
- self.translation_vectors[view_index],
- self.camera_matrix,
- self.distortion_coefficients)
+ projected_image_points, jacobian = cv2.projectPoints(objectPoints= self.object_points_views[view_index],
+ rvec= self.rotation_vectors[view_index],
+ tvec= self.translation_vectors[view_index],
+ cameraMatrix= self.camera_matrix,
+ distCoeffs= self.distortion_coefficients,
+ aspectRatio=self.image_size[0]/self.image_size[1]
+ )
self.reprojection_error_by_view.append(
np.mean(np.abs(self.image_points_views[view_index] - np.squeeze(projected_image_points))))