Free Motion Capture for Everyone πβ¨
TypeScript
optimizer running, but needs better setup I think
jonmatthis582cd295f933
diff --git a/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/run_anipose_calibration_algorithm.py b/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/run_anipose_calibration_algorithm.pyindex 746e4809..84c4d6e3 100644--- a/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/run_anipose_calibration_algorithm.py+++ b/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/run_anipose_calibration_algorithm.py@@ -16,13 +16,13 @@ from skellycam import CameraIdfrom tqdm import trangefrom freemocap.pipelines.calibration_pipeline.calibration_numpy_types import \- PixelPoints2DByCamera, CameraExtrinsicsMatrixByCamera+ ImagePoints2DByCamera, CameraExtrinsicsMatrixByCameralogger = logging.getLogger(__name__)@jit(nopython=True, parallel=False)-def anipose_triangulate_simple(points2d_by_camera: PixelPoints2DByCamera,+def anipose_triangulate_simple(points2d_by_camera: ImagePoints2DByCamera,camera_matrices_by_camera: CameraExtrinsicsMatrixByCamera) -> np.ndarray:"""Triangulate 3D point from 2D points and camera matrices using the Direct Linear Transform (DLT) method.diff --git a/freemocap/pipelines/calibration_pipeline/calibration_aggregation_node.py b/freemocap/pipelines/calibration_pipeline/calibration_aggregation_node.pyindex 1691e57c..31414357 100644--- a/freemocap/pipelines/calibration_pipeline/calibration_aggregation_node.py+++ b/freemocap/pipelines/calibration_pipeline/calibration_aggregation_node.py@@ -19,7 +19,7 @@ logger = logging.getLogger(__name__)class CalibrationAggregationLayerOutputData(BaseAggregationLayerOutputData):- multi_camera_calibration_estimate:MultiCameraCalibrationEstimate|None = None+ multi_camera_calibration_estimate: MultiCameraCalibrationEstimate | None = Nonedata: object = None@@ -41,24 +41,24 @@ class CalibrationAggregationProcessNode(BaseAggregationNode):output_queue: Queue,all_ready_events: dict[CameraId | str, multiprocessing.Event],shutdown_event: multiprocessing.Event,- use_thread: bool = False):+ use_thread: bool = False):if use_thread:worker = Thread(target=cls._run,- kwargs=dict(config=config,- input_queues=input_queues,- output_queue=output_queue,- all_ready_events=all_ready_events,- shutdown_event=shutdown_event)- )+ kwargs=dict(config=config,+ input_queues=input_queues,+ output_queue=output_queue,+ all_ready_events=all_ready_events,+ shutdown_event=shutdown_event)+ )else:worker = Process(target=cls._run,- kwargs=dict(config=config,- input_queues=input_queues,- output_queue=output_queue,- all_ready_events=all_ready_events,- shutdown_event=shutdown_event)- )+ kwargs=dict(config=config,+ input_queues=input_queues,+ output_queue=output_queue,+ all_ready_events=all_ready_events,+ shutdown_event=shutdown_event)+ )return cls(config=config,process=worker,input_queues=input_queues,@@ -80,7 +80,6 @@ class CalibrationAggregationProcessNode(BaseAggregationNode):camera_id ininput_queues.keys()}-multi_camera_calibrator = MultiCameraCalibrator.from_camera_ids(camera_ids=list(input_queues.keys()))try:diff --git a/freemocap/pipelines/calibration_pipeline/calibration_numpy_types.py b/freemocap/pipelines/calibration_pipeline/calibration_numpy_types.pyindex e56e9500..4ffaf1e9 100644--- a/freemocap/pipelines/calibration_pipeline/calibration_numpy_types.py+++ b/freemocap/pipelines/calibration_pipeline/calibration_numpy_types.py@@ -1,10 +1,13 @@import numpy as npfrom numpydantic import NDArray, Shape+ReprojectionError = float-PixelPoints2D = NDArray[Shape["* n_points, 2 pixelx_pixely"], np.float64]-PixelPoint2DByCamera = NDArray[Shape["* n_cams, 2 pixelx_pixely"], np.float64] # e.g. Image coordinates of a point in 3d space projected onto each camera+ImagePoint2D = NDArray[Shape["2 pixelx_pixely"], np.float64]+ImagePoints2D = NDArray[Shape["* n_points, 2 pixelx_pixely"], np.float64]+ImagePoints2DByCamera = NDArray[Shape["* n_cams, * n_points, 2 pixelx_pixely"], np.float64] # e.g. Image coordinates of a point in 3d space projected onto each camera+ObjectPoint3D = NDArray[Shape["3 xyz"], np.float64]ObjectPoints3D = NDArray[Shape["* n_points, 3 xyz"], np.float64]PointIds = NDArray[Shape["* n_points"], np.int64]@@ -16,16 +19,13 @@ RotationVectorsByCamera = NDArray[Shape["* n_cams, 3"], np.float64]TranslationVectorsByCamera = NDArray[Shape["* n_cams, 3"], np.float64]CameraMatrixArray = NDArray[Shape["3 rows, 3 columns"], np.float64]-CameraExtrinsicsMatrix = NDArray[Shape["4 rows, 4 columns"], np.float64]+CameraExtrinsicsMatrix = NDArray[Shape["3 rows, 4 columns"], np.float64]CameraMatrixByCamera = NDArray[Shape["* n_cams, 3 rows, 3 columns"], np.float64]CameraExtrinsicsMatrixByCamera = NDArray[Shape["* n_cams, 4 rows, 4 columns"], np.float64]TransformationMatrixArray = 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 coordinatesCameraDistortionCoefficientsArray = NDArray[Shape["4-14 k1_k2_p1_p2_k3_k4_k5_k6"], np.float64] # Can be 4,5,8,12, or 14 elements-ReprojectionError = NDArray[Shape["* n_points"], np.float64]--ExtrinsicsParameters = NDArray[Shape["6 translation_rotation"], np.float64] # 3 for rotation, 3 for translationIntrinsicsParameters = NDArray[Shape["*, ..."], np.float64] # focal length, variable number of distortion coefficients (2, 4, 5, 8, 12, or 14)@@ -33,4 +33,7 @@ IntrinsicsParameters = NDArray[Shape["*, ..."], np.float64] # focal length, varExtrinsicsParametersByCamera = NDArray[Shape["* n_cams, 6 translation_rotation"], np.float64]IntrinsicsParametersByCamera = NDArray[Shape["* n_cams, * focal_length_distortion"], np.float64]-JacobianMatrixArray = NDArray[Shape["* m_n_error_function_returns, * n_parameters"], np.float64]\ No newline at end of file+JacobianMatrixArray = NDArray[Shape["* m_n_error_function_returns, * n_parameters"], np.float64]+++SparseBundleOptimizerGuess1D = NDArray[Shape["* n_parameters"], np.float64]\ No newline at end of filediff --git a/freemocap/pipelines/calibration_pipeline/camera_math_models.py b/freemocap/pipelines/calibration_pipeline/camera_math_models.pynew file mode 100644index 00000000..9979d305--- /dev/null+++ b/freemocap/pipelines/calibration_pipeline/camera_math_models.py@@ -0,0 +1,234 @@+import cv2+import numpy as np+from numpydantic import NDArray, Shape+from pydantic import BaseModel, model_validator+from scipy.spatial.transform import Rotation as R++from freemocap.pipelines.calibration_pipeline.calibration_numpy_types import QuaternionArray, RotationVectorArray, \+ RotationMatrixArray, TranslationVectorArray, CameraExtrinsicsMatrix, CameraDistortionCoefficientsArray, \+ CameraMatrixArray+++def karcher_mean_quaternions(quaternions: list[QuaternionArray], tol=1e-9, max_iterations=100):+ # Normalize the quaternions to ensure they are unit quaternions++ quaternions = np.array([q / np.linalg.norm(q) for q in quaternions])++ # Start with an initial guess (the first quaternion)+ mean_quat = quaternions[0]++ for _ in range(max_iterations):+ # Compute the tangent vectors (logarithm map)+ errors = [R.from_quat(q).inv() * R.from_quat(mean_quat) for q in quaternions]+ errors = [e.as_rotvec() for e in errors]++ # Compute the average error vector+ mean_error = np.mean(errors, axis=0)++ # Check for convergence+ if np.linalg.norm(mean_error) < tol:+ break++ # Update the mean (exponential map)+ mean_rot = R.from_rotvec(mean_error) * R.from_quat(mean_quat)+ mean_quat = mean_rot.as_quat(canonical=False)++ return mean_quat+++class RotationVector(BaseModel):+ vector: RotationVectorArray+ reference_frame: str++ @property+ def as_rotation_matrix(self) -> RotationMatrixArray:+ return cv2.Rodrigues(self.vector)[0]++ @property+ def as_quaternion(self) -> QuaternionArray:+ return R.from_matrix(self.as_rotation_matrix).as_quat(canonical=False)++ @classmethod+ def mean_from_rotation_vectors(cls, rotation_vectors: list["RotationVector"]) -> "RotationVector":+ if not all([rv.reference_frame == rotation_vectors[0].reference_frame for rv in rotation_vectors]):+ raise ValueError("All rotation vectors must have the same reference frame!")+ quaternions = [rv.as_quaternion for rv in rotation_vectors]++ # Compute the Karcher mean of the quaternions+ mean_quat = karcher_mean_quaternions(quaternions)++ # Convert the mean quaternion back to a rotation vector+ mean_rotation = R.from_quat(mean_quat)+ mean_rotation_vector = mean_rotation.as_rotvec()++ # Assuming all rotation vectors have the same reference frame+ reference_frame = rotation_vectors[0].reference_frame++ return cls(vector=mean_rotation_vector,+ reference_frame=reference_frame)+++class TranslationVector(BaseModel):+ vector: TranslationVectorArray+ reference_frame: str++ @classmethod+ def mean_from_translation_vectors(cls, translation_vectors: list["TranslationVector"]) -> "TranslationVector":+ if not all([tv.reference_frame == translation_vectors[0].reference_frame for tv in translation_vectors]):+ raise ValueError("All translation vectors must have the same reference frame!")+ mean_translation_vector = np.mean([tv.vector for tv in translation_vectors], axis=0)++ # Assuming all rotation vectors have the same reference frame+ reference_frame = translation_vectors[0].reference_frame++ return cls(vector=mean_translation_vector,+ reference_frame=reference_frame)+++class TransformationMatrix(BaseModel):+ matrix: NDArray[Shape["4, 4"], np.float64]+ reference_frame: str++ @classmethod+ def from_extrinsics(cls, extrinsics_matrix: CameraExtrinsicsMatrix, reference_frame: str):+ rotation_matrix = np.squeeze(extrinsics_matrix[:3, :3])+ translation_vector = np.squeeze(extrinsics_matrix[:3, 3])+ return cls.from_rotation_translation(rotation_vector=RotationVector(vector=np.squeeze(cv2.Rodrigues(rotation_matrix)[0]),+ reference_frame=reference_frame),+ translation_vector=TranslationVector(vector=translation_vector,+ reference_frame=reference_frame))++ @classmethod+ def from_rotation_translation(cls,+ rotation_vector: RotationVector,+ translation_vector: TranslationVector):+ if rotation_vector.reference_frame != translation_vector.reference_frame:+ raise ValueError("Rotation and translation vectors must be in the same reference frame")+ rotation_matrix = rotation_vector.as_rotation_matrix+ transformation_matrix = np.eye(4)+ transformation_matrix[:3, :3] = rotation_matrix+ transformation_matrix[:3, 3] = translation_vector.vector+ return cls(matrix=transformation_matrix, reference_frame=translation_vector.reference_frame)++ @classmethod+ def mean_from_transformation_matrices(cls, transformation_matrices: list[+ "TransformationMatrix"]) -> "TransformationMatrix":+ if not all(+ [tm.reference_frame == transformation_matrices[0].reference_frame for tm in transformation_matrices]):+ raise ValueError("All transformation matrices must have the same reference frame!")++ rotation_vectors = [tm.rotation_vector for tm in transformation_matrices]+ translation_vectors = [tm.translation_vector for tm in transformation_matrices]++ mean_rotation_vector = RotationVector.mean_from_rotation_vectors(rotation_vectors)+ mean_translation_vector = TranslationVector.mean_from_translation_vectors(translation_vectors)++ return cls.from_rotation_translation(rotation_vector=mean_rotation_vector,+ translation_vector=mean_translation_vector)++ @property+ def rotation_matrix(self) -> NDArray[Shape["3, 3"], np.float32]:+ return self.matrix[:3, :3]++ @property+ def translation_vector(self) -> TranslationVector:+ return TranslationVector(vector=self.matrix[:3, 3], reference_frame=self.reference_frame)++ @property+ def rotation_vector(self) -> RotationVector:+ return RotationVector(vector=np.squeeze(cv2.Rodrigues(self.rotation_matrix)[0]), reference_frame=self.reference_frame)++ @property+ def extrinsics_matrix(self) -> CameraExtrinsicsMatrix:+ return self.matrix[:3, :]+++ def get_inverse(self):+ inverse_rotation_matrix = self.rotation_matrix.T+ inverse_translation_vector = -inverse_rotation_matrix @ self.translation_vector.vector++ inverse_matrix = np.eye(4)+ inverse_matrix[:3, :3] = inverse_rotation_matrix+ inverse_matrix[:3, 3] = inverse_translation_vector++ return TransformationMatrix(matrix=inverse_matrix, reference_frame=self.reference_frame)++ def __matmul__(self, other: "TransformationMatrix") -> "TransformationMatrix":+ if not isinstance(other, TransformationMatrix):+ raise TypeError(+ "Unsupported operand type(s) for @: 'TransformationMatrix' and '{}'".format(type(other).__name__))+ # Perform matrix multiplication and use self's reference frame+ return TransformationMatrix(matrix=self.matrix @ other.matrix, reference_frame=self.reference_frame)+++ def __str__(self) -> str:+ return (f"\treference_frame={self.reference_frame},\n"+ f"\tmatrix=\n"+ f"\t\t{self.matrix[0, :]:.3f},\n"+ f"\t\t{self.matrix[1, :]:.3f},\n"+ f"\t\t{self.matrix[2, :]:.3f},\n"+ f"\t\t{self.matrix[3, :]:.3f},\n")+++class CameraDistortionCoefficients(BaseModel):+ """+ https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html+ Distortion coefficients for the camera, must be either 4, 5, 8, 12, or 14 values.++ 0-3 are k1, k2, p1, p2+ 4 is k3+ 5-7 are k4, k5, k6+ 8-11 are s1, s2, s3, s4+ 12-13 are Ο1, Ο2 (Ο like tau)++ `k` values refer to radial distortion coefficients+ `p` values refer to tangential distortion coefficients+ `s` values refer to thin prism distortion coefficients+ `Ο` values refer to x/y values of the 'tilted sensor' model++ NOTE - RECOMMEND USING 5 VALUES, things get weird with more than 5+ """+ coefficients: CameraDistortionCoefficientsArray++ @model_validator(mode="after")+ def validate(self):+ if len(self.coefficients) not in [4, 5, 8, 12, 14]:+ raise ValueError("Invalid number of distortion coefficients. Must be 4, 5, 8, 12, or 14.")+ return self+++class CameraMatrix(BaseModel):+ matrix: CameraMatrixArray++ @classmethod+ def from_image_size(cls, image_size: tuple[int, ...]):+ camera_matrix = np.eye(3)+ camera_matrix[0, 2] = image_size[0] / 2 # x_center+ camera_matrix[1, 2] = image_size[1] / 2 # y_center+ return cls(matrix=camera_matrix)++ @model_validator(mode="after")+ def validate(self):+ if self.matrix.shape != (3, 3):+ raise ValueError("Camera matrix must be 3x3")+ return self++ @property+ def focal_length(self) -> float:+ return self.matrix[0, 0]++ @property+ def focal_length_x(self) -> float:+ return self.matrix[0, 0]++ @property+ def focal_length_y(self) -> float:+ return self.matrix[1, 1]++ @property+ def focal_length_xy(self) -> float:+ return (self.focal_length_x + self.focal_length_y) / 2++ @property+ def principal_point(self) -> tuple[float, float]:+ return self.matrix[0, 2], self.matrix[1, 2]diff --git a/freemocap/pipelines/calibration_pipeline/least_squares_optimizer.py b/freemocap/pipelines/calibration_pipeline/least_squares_optimizer.pyindex 3387341a..9c4767dd 100644--- a/freemocap/pipelines/calibration_pipeline/least_squares_optimizer.py+++ b/freemocap/pipelines/calibration_pipeline/least_squares_optimizer.py@@ -1,108 +1,173 @@-from abc import ABC, abstractmethod-from typing import TYPE_CHECKING-+import numpy as npfrom pydantic import BaseModel+from pydantic import model_validatorfrom scipy import optimizefrom scipy.sparse import dok_matrixfrom skellycam import CameraId+from typing_extensions import Selffrom freemocap.pipelines.calibration_pipeline.calibration_numpy_types import \- JacobianMatrixArray+ ImagePoints2D, ImagePoint2D, CameraExtrinsicsMatrix+from freemocap.pipelines.calibration_pipeline.camera_math_models import TransformationMatrixfrom freemocap.pipelines.calibration_pipeline.shared_view_accumulator import MultiCameraTargetView, MultiFrameNumber-from freemocap.pipelines.calibration_pipeline.single_camera_calibrator import CameraIntrinsicsEstimate, \- TransformationMatrix--if TYPE_CHECKING:- pass-+from freemocap.pipelines.calibration_pipeline.single_camera_calibrator import CameraIntrinsicsEstimate+from freemocap.pipelines.calibration_pipeline.triangulate_points import undistort_points, \+ calculate_reprojection_error, triangulate_point-class LeastSquaresOptimizerStartingGuess(BaseModel):- pass- @abstractmethod- def to_list(self) -> list[float]:- pass+class SparseBundleOptimizer(BaseModel):+ principal_camera_id: CameraId+ initial_guess: dict[CameraId, CameraExtrinsicsMatrix]+ image_points_by_camera: dict[CameraId, list[ImagePoint2D]]+ camera_intrinsics: dict[CameraId, CameraIntrinsicsEstimate]+ image_sizes: dict[CameraId, tuple[int, int]]+ @classmethod+ def create(cls,+ principal_camera_id: CameraId,+ camera_extrinsics_by_camera_id: dict[CameraId, CameraExtrinsicsMatrix],+ multi_camera_target_views: dict[MultiFrameNumber, MultiCameraTargetView],+ camera_intrinsics: dict[CameraId, CameraIntrinsicsEstimate],+ image_sizes: dict[CameraId, tuple[int, int]]+ ) -> "SparseBundleOptimizer":+ image_points_by_camera: dict[CameraId, list[ImagePoint2D]] = {camera_id: [] for camera_id in+ camera_intrinsics.keys()}+ for frame_number, target_view in multi_camera_target_views.items():+ for camera_id, image_points in target_view.image_points_by_camera.items():+ image_points_by_camera[camera_id].extend(image_points)+ return cls(+ image_points_by_camera=image_points_by_camera,+ camera_intrinsics=camera_intrinsics,+ initial_guess=camera_extrinsics_by_camera_id,+ principal_camera_id=principal_camera_id,+ image_sizes=image_sizes+ )-class LeastSquaresOptimizerABC(BaseModel, ABC):- initial_guess: object- loss: str = "linear"- maximum_number_function_evals: int = 1000- threshold: float = 50.0- function_tolerance: float = 1e-4- jacobian_sparsity_matrix: JacobianMatrixArray | None = None+ @model_validator(mode='after')+ def validate(self) -> Self:+ # make sure the principal camera is in all the keys, along with at least one other camera+ if self.principal_camera_id not in self.initial_guess or len(self.initial_guess) < 2:+ raise ValueError("Principal camera must be in the initial guess and there must be at least two cameras")+ camera_ids = self.initial_guess.keys()+ if not all([camera_ids == self.image_points_by_camera.keys(),+ camera_ids == self.camera_intrinsics.keys(),+ camera_ids == self.image_points_by_camera.keys(),+ camera_ids == self.image_sizes.keys()]):+ raise ValueError("All camera ids must be the same across all inputs")+ for camera_id, image_points in self.image_points_by_camera.items():+ if len(image_points) < 2:+ raise ValueError(f"Camera {camera_id} must have at least two image points")+ if len(image_points) != self.number_of_points:+ raise ValueError(+ f"Camera {camera_id} must have the same number of image points as the principal camera")+ return self- @classmethod- @abstractmethod- def create(cls, **kwargs) -> "LeastSquaresOptimizerABC":- pass+ @property+ def number_of_optimization_parameters(self) -> int:+ return len(self.starting_guess_vector)@property- def starting_guess(self) -> list[float]:- return self.initial_guess_model+ def number_of_parameters_per_camera(self) -> int:+ return self.initial_guess[self.principal_camera_id].size- @abstractmethod- def error_function(self, current_guess: list[float], **kwargs) -> float:- pass+ @property+ def number_of_points(self) -> int:+ return len(self.image_points_by_camera[self.principal_camera_id])- @abstractmethod- def get_jacobian_sparse_matrix(self, current_guess: list[float], **kwargs) -> dok_matrix:- pass+ @property+ def starting_guess_vector(self) -> list[float]:+ starting_guess = []+ for camera_id, extrinsics in self.initial_guess.items():+ starting_guess.extend(extrinsics.flatten().tolist())+ return starting_guessdef optimize(self) -> optimize.OptimizeResult:+# Set up optimization optionsreturn optimize.least_squares(fun=self.error_function,- x0=self.starting_guess,- jac_sparsity=self.get_jacobian_sparse_matrix(self.starting_guess),- f_scale=self.threshold,+ x0=self.starting_guess_vector,+ # jac_sparsity=self.get_jacobian_sparse_matrix(),+ jac="3-point", # TODO- could prob use `callable` and calc the jacobian dynamically+ f_scale=50.0, # TODO - Not sure where this number comes fromx_scale="jac",- loss=self.loss,- ftol=self.function_tolerance,+ loss="soft_l1",#"linear", # TODO - try `soft_l1` or `cauchy`+ # ftol=1e-4,method="trf",verbose=2,- max_nfev=self.maximum_number_function_evals,- kwargs=self.error_function_input.model_dump()- )-+ bounds=(-np.inf, np.inf), # TODO - set bounds to reasonable values+ max_nfev=1000,+ kwargs=dict(camera_intrinsics=self.camera_intrinsics,+ image_points_by_camera=self.image_points_by_camera,+ principal_camera_id=self.principal_camera_id,+ image_sizes=self.image_sizes),-class SparseBundleOptimizer(LeastSquaresOptimizerABC):- principal_camera_id: CameraId- initial_guess: dict[CameraId, TransformationMatrix]- multi_camera_target_views: dict[MultiFrameNumber, MultiCameraTargetView]- camera_intrinsics: dict[CameraId, CameraIntrinsicsEstimate]-- @classmethod- def create(cls,- principal_camera_id: CameraId,- camera_extrinsic_transforms_by_camera_id: dict[CameraId, TransformationMatrix],- multi_camera_target_views: dict[MultiFrameNumber, MultiCameraTargetView],- camera_intrinsics: dict[CameraId, CameraIntrinsicsEstimate]- ) -> "SparseBundleOptimizer":- return cls(- multi_camera_target_views=multi_camera_target_views,- camera_intrinsics=camera_intrinsics,- initial_guess=camera_extrinsic_transforms_by_camera_id,- principal_camera_id=principal_camera_id)def error_function(self,current_guess: list[float],- **kwargs- ) -> float:- try:- camera_matrices =kwargs['camera_matrices']- multi_camera_target_views = kwargs['multi_camera_target_views']- principal_camera_id = kwargs['principal_camera_id']- except KeyError:- raise ValueError("Missing required keyword arguments")- # reform 'current_guess' into camera extrinsics matricies+ camera_intrinsics: dict[CameraId, CameraIntrinsicsEstimate],+ image_points_by_camera: dict[CameraId, ImagePoints2D],+ principal_camera_id: CameraId,+ image_sizes: dict[CameraId, tuple[int, int]]+ ) -> list[float]:+ # reform 'current_guess' into camera extrinsics matricies+ camera_ids = list(self.camera_intrinsics.keys())+ camera_transforms: dict[CameraId, TransformationMatrix] = {}+ camera_extrinsics: dict[CameraId, CameraExtrinsicsMatrix] = {}+ undistored_image_points: dict[CameraId, ImagePoints2D] = {}+ for camera_index, camera_id in enumerate(camera_ids):+ # split the 1d vector by camera based on `self.number_of_parameters_per_camera`+ starting_index = camera_index * self.number_of_parameters_per_camera+ ending_index = (camera_index + 1) * self.number_of_parameters_per_camera+ camera_transforms[camera_id] = TransformationMatrix.from_extrinsics(+ extrinsics_matrix=np.array(current_guess[starting_index:ending_index]).reshape(3, 4),+ reference_frame=f"camera-{principal_camera_id}")+ camera_extrinsics[camera_id] = camera_transforms[camera_id].extrinsics_matrix+ # undistort the image points for each camera using the camera intrinsics+ undistored_image_points[camera_id] = undistort_points(+ points2d=np.asarray(image_points_by_camera[camera_id]),+ camera_intrinsics=camera_intrinsics[camera_id])+ # re-scale from normalized image coordinates to pixel coordinates+ # undistored_image_points[camera_id][:, 0] *= image_sizes[camera_id][0]+ # undistored_image_points[camera_id][:, 1] *= image_sizes[camera_id][1]# triangulate 3D points from 2D points using camera extrinsics and intrinsics (provided as kwargs)-- # calculate reprojection error- error = 0.0- return error-- def get_jacobian_sparse_matrix(self, current_guess: list[float], **kwargs) -> dok_matrix:- pass+ points3d = []+ for point_index in range(self.number_of_points):+ # triangulate the 3D point from the 2D points+ image_point_by_camera = {camera_id: undistored_image_points[camera_id][point_index]+ for camera_id in camera_ids}+ points3d.append(triangulate_point(image_point_by_camera=image_point_by_camera,+ camera_extrinsics=camera_extrinsics))++ # calculate reprojection error for each point+ reprojection_error_by_point, _ = calculate_reprojection_error(object_points=np.asarray(points3d),+ image_points_by_camera=undistored_image_points,+ camera_transforms=camera_transforms,+ camera_intrinsics=camera_intrinsics,+ image_sizes=image_sizes)++ return np.mean(reprojection_error_by_point)++ def get_jacobian_sparse_matrix(self) -> dok_matrix:+ jacobian_matrix = np.ones((self.number_of_points, self.number_of_optimization_parameters))++ # TODO - Implement the logic to populate the jacobian_matrix based on the current guess and image points+ # basically for each row (point2d) on the jacobian matrix, we need to populate the columns (parameters) that are affected by that point+ # so the parameters for the camea that can't see each point should be zero++ for point_index in range(self.number_of_points):+ row_values = []+ num_cams_that_can_see_point = 0+ for camera_id, image_points in self.image_points_by_camera.items():+ if np.isnan(image_points[point_index][0]) or np.isnan(image_points[point_index][1]):+ row_values.append(np.zeros(self.number_of_parameters_per_camera))+ else:+ num_cams_that_can_see_point += 1+ row_values.append(np.ones(self.number_of_parameters_per_camera))+ if num_cams_that_can_see_point < 2:+ row_values = [np.zeros(self.number_of_parameters_per_camera) for _ in range(len(row_values))]+ jacobian_matrix[point_index] = np.concatenate(row_values)++ return dok_matrix(jacobian_matrix).tocsr()diff --git a/freemocap/pipelines/calibration_pipeline/multi_camera_calibration/anipose_calibration_optimizer.py b/freemocap/pipelines/calibration_pipeline/multi_camera_calibration/anipose_calibration_optimizer.pyindex 9cef4498..6fe7823f 100644--- a/freemocap/pipelines/calibration_pipeline/multi_camera_calibration/anipose_calibration_optimizer.py+++ b/freemocap/pipelines/calibration_pipeline/multi_camera_calibration/anipose_calibration_optimizer.py@@ -12,8 +12,8 @@ from tqdm import trangefrom freemocap.core_processes.capture_volume_calibration.anipose_camera_calibration.run_anipose_calibration_algorithm import \anipose_triangulate_simple, remap_ids-from freemocap.pipelines.calibration_pipeline.calibration_numpy_types import PixelPoints2D, \- ObjectPoints3D, ExtrinsicsParameters, IntrinsicsParameters, ReprojectionError, PixelPoints2DByCamera, PointIds, \+from freemocap.pipelines.calibration_pipeline.calibration_numpy_types import ImagePoints2D, \+ ObjectPoints3D, ExtrinsicsParameters, IntrinsicsParameters, ReprojectionErrorByPoint, ImagePoints2DByCamera, PointIds, \RotationVectorsByCamera, TranslationVectorsByCamerafrom freemocap.pipelines.calibration_pipeline.multi_camera_calibration.calibration_utilities import \calculate_error_bounds, transform_points, construct_camera_extrinsics_matrix, \@@ -27,7 +27,7 @@ class SingleCameraCalibrationHelper(BaseModel):calibrator: SingleCameraCalibrator@property- def pixel_points2d(self) -> PixelPoints2D:+ def pixel_points2d(self) -> ImagePoints2D:return self.calibrator.image_points_viewsdef get_optimizable_parameters(self) -> tuple[ExtrinsicsParameters, IntrinsicsParameters]:@@ -58,7 +58,7 @@ class SingleCameraCalibrationHelper(BaseModel):distortion_coefficients[1] = intrinsics_parameters[2]self.distortion_coefficients = distortion_coefficients- def undistort_points(self, points2d: PixelPoints2D) -> PixelPoints2D:+ def undistort_points(self, points2d: ImagePoints2D) -> ImagePoints2D:"""Undistorts 2D points using the camera's intrinsic parameters and distortion coefficients.@@ -79,7 +79,7 @@ class SingleCameraCalibrationHelper(BaseModel):self.distortion_coefficients.astype("float64"))return undistorted_points2d.reshape(shape)- def project_3d_to_2d(self, points3d: ObjectPoints3D) -> PixelPoints2D:+ def project_3d_to_2d(self, points3d: ObjectPoints3D) -> ImagePoints2D:if not points3d.shape[-1] == 3:raise ValueError("points3d must have shape (N, 3)")points3d = points3d.reshape(-1, 1, 3)@@ -95,14 +95,14 @@ class SingleCameraCalibrationHelper(BaseModel):return projected_2d_points- def single_camera_reprojection_error(self, points3d: ObjectPoints3D, points2d: PixelPoints2D) -> ReprojectionError:+ def single_camera_reprojection_error(self, points3d: ObjectPoints3D, points2d: ImagePoints2D) -> ReprojectionErrorByPoint: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_planeclass MultiCameraCalibrationInputData(BaseModel):- pixel_points2d: PixelPoints2DByCamera+ pixel_points2d: ImagePoints2DByCameraobject_points3d: ObjectPoints3Dpoints_ids: PointIdsrotation_vectors: RotationVectorsByCameradiff --git a/freemocap/pipelines/calibration_pipeline/multi_camera_calibrator.py b/freemocap/pipelines/calibration_pipeline/multi_camera_calibrator.pyindex 2dd3198a..5234df0f 100644--- a/freemocap/pipelines/calibration_pipeline/multi_camera_calibrator.py+++ b/freemocap/pipelines/calibration_pipeline/multi_camera_calibrator.py@@ -11,7 +11,8 @@ from freemocap.pipelines.calibration_pipeline.shared_view_accumulator import Shalogger = logging.getLogger(__name__)from freemocap.pipelines.calibration_pipeline.single_camera_calibrator import SingleCameraCalibrator, \- TransformationMatrix+ CameraIntrinsicsEstimate+from freemocap.pipelines.calibration_pipeline.camera_math_models import TransformationMatrixclass MultiCameraCalibrationEstimate(BaseModel):@@ -19,7 +20,7 @@ class MultiCameraCalibrationEstimate(BaseModel):camera_extrinsic_transforms_by_camera_id: dict[CameraId, TransformationMatrix]def to_list(self):- return [transform.as_extrinsics_matrix.flatten() for transform in+ return [transform.extrinsics_matrix.flatten() for transform inself.camera_extrinsic_transforms_by_camera_id.values()]@@ -40,8 +41,14 @@ class MultiCameraCalibrator(BaseModel):return all(single_camera_calibrator.has_calibration for single_camera_calibrator inself.single_camera_calibrators.values()) and self.multi_camera_calibration_estimate is not None+ @property+ def camera_intrinsics(self) -> dict[CameraId, CameraIntrinsicsEstimate]:+ return {camera_id: single_camera_calibrator.camera_intrinsics_estimate for camera_id, single_camera_calibrator+ in+ self.single_camera_calibrators.items()}+@classmethod- def from_camera_ids(cls, camera_ids: list[CameraId], principal_camera_id: CameraId | None = None):+ def from_camera_ids(cls, camera_ids: list[CameraId], principal_camera_id: CameraId):return cls(principal_camera_id=principal_camera_id if principal_camera_id is not None else min(camera_ids),camera_id_to_index={camera_id: index for index, camera_id in enumerate(camera_ids)},shared_view_accumulator=SharedViewAccumulator.create(camera_ids=camera_ids),@@ -110,20 +117,20 @@ class MultiCameraCalibrator(BaseModel):camera_pair_secondary_camera_transform_estimates)self.spare_bundle_optimizer = SparseBundleOptimizer.create(principal_camera_id=self.principal_camera_id,- camera_extrinsic_transforms_by_camera_id=transform_to_principal_camera_by_camera,- camera_intrinsics={- camera_id: calibrator.camera_intrinsics_estimate- for camera_id, calibrator in- self.single_camera_calibrators.items()},- multi_camera_target_views={- camera_id: calibrator.charuco_observations for- camera_id, calibrator in- self.single_camera_calibrators.items()},+ camera_extrinsics_by_camera_id={+ camera_id: transform.extrinsics_matrix+ for camera_id, transform in transform_to_principal_camera_by_camera.items()},+ camera_intrinsics=self.camera_intrinsics,+ multi_camera_target_views=self.shared_view_accumulator.multi_camera_target_views,+ image_sizes={camera_id: calibrator.image_size for+ camera_id, calibrator in+ self.single_camera_calibrators.items()})optimization_result = self.spare_bundle_optimizer.optimize()return self.multi_camera_calibration_estimate- def _calculate_camera_to_principal_camera_transforms(self, camera_pair_secondary_camera_transform_estimates) -> dict[CameraId, TransformationMatrix]:+ def _calculate_camera_to_principal_camera_transforms(self, camera_pair_secondary_camera_transform_estimates) -> \+ dict[CameraId, TransformationMatrix]:transform_to_principal_camera_by_camera = {self.principal_camera_id: TransformationMatrix(matrix=np.eye(4),reference_frame=f"camera {self.principal_camera_id}")}transform_to_principal_camera_by_camera.update(@@ -188,7 +195,6 @@ if __name__ == "__main__":raise FileNotFoundError(f"File not found: {pickle_path}")loaded: MultiCameraCalibrator = pickle.load(open(pickle_path, "rb"))loaded.run_multi_camera_optimization()- print(loaded)# save out# with open(pickle_path, "wb") as f:diff --git a/freemocap/pipelines/calibration_pipeline/shared_view_accumulator.py b/freemocap/pipelines/calibration_pipeline/shared_view_accumulator.pyindex 92424acc..88c7907b 100644--- a/freemocap/pipelines/calibration_pipeline/shared_view_accumulator.py+++ b/freemocap/pipelines/calibration_pipeline/shared_view_accumulator.py@@ -1,8 +1,9 @@+import numpy as npfrom pydantic import BaseModel, Fieldfrom skellycam import CameraIdfrom pydantic import model_validatorfrom freemocap.pipelines.calibration_pipeline.calibration_camera_node_output_data import CalibrationCameraNodeOutputData-+from freemocap.pipelines.calibration_pipeline.calibration_numpy_types import ImagePoint2DMultiFrameNumber = intclass CameraPair(BaseModel):@@ -44,10 +45,28 @@ class CameraPairTargetView(BaseModel):raise ValueError(f"other_camera_id {self.camera_pair.other_camera_id} not in camera_node_output_by_camera keys {self.camera_node_output_by_camera.keys()}")return self++PointIndex = intclass MultiCameraTargetView(BaseModel):multi_frame_number: MultiFrameNumbercamera_node_output_by_camera: dict[CameraId, CalibrationCameraNodeOutputData]+ @model_validator(mode='after')+ def validate(self):+ if len(self.camera_node_output_by_camera) < 2:+ raise ValueError(f"camera_node_output_by_camera must have at least 2 cameras, got {len(self.camera_node_output_by_camera)}")+ if not all([output.frame_number == self.multi_frame_number for output in self.camera_node_output_by_camera.values()]):+ raise ValueError(f"multi_frame_number {self.multi_frame_number} does not match all camera_node_output_by_camera multi_frame_numbers")+ return self++ @property+ def image_points_by_camera(self) -> dict[CameraId, list[ImagePoint2D]]:+ image_points_by_camera = {camera_id: [] for camera_id in self.camera_node_output_by_camera.keys()}+ for camera_id, camera_node_output in self.camera_node_output_by_camera.items():+ image_points_by_camera[camera_id].extend(list(camera_node_output.charuco_observation.detected_charuco_corners_in_full_array))++ return image_points_by_camera+class SharedViewAccumulator(BaseModel):"""Keeps track of the data feeds from each camera, and keeps track of the frames where each can see the calibration target,diff --git a/freemocap/pipelines/calibration_pipeline/single_camera_calibrator.py b/freemocap/pipelines/calibration_pipeline/single_camera_calibrator.pyindex 7d31cd49..2834fbe1 100644--- a/freemocap/pipelines/calibration_pipeline/single_camera_calibrator.py+++ b/freemocap/pipelines/calibration_pipeline/single_camera_calibrator.py@@ -2,238 +2,18 @@ import loggingimport cv2import numpy as np-from numpydantic import NDArray, Shape-from pydantic import BaseModel, Field, model_validator+from pydantic import BaseModel, Fieldfrom skellycam import CameraIdfrom skellytracker.trackers.charuco_tracker.charuco_observation import CharucoObservations, CharucoObservation, \AllCharucoCorners3DByIdInObjectCoordinates, AllArucoCorners3DByIdInObjectCoordinatesfrom freemocap.pipelines.calibration_pipeline.calibration_numpy_types import ObjectPoints3D, \- PixelPoints2D, RotationVectorArray, TranslationVectorArray, CameraDistortionCoefficientsArray, CameraMatrixArray, \- CameraExtrinsicsMatrix, RotationMatrixArray, QuaternionArray+ ImagePoints2D+from freemocap.pipelines.calibration_pipeline.camera_math_models import RotationVector, TranslationVector, \+ TransformationMatrix, CameraDistortionCoefficients, CameraMatrixlogger = logging.getLogger(__name__)-from scipy.spatial.transform import Rotation as R----def karcher_mean_quaternions(quaternions: list[QuaternionArray], tol=1e-9, max_iterations=100):- # Normalize the quaternions to ensure they are unit quaternions-- quaternions = np.array([q / np.linalg.norm(q) for q in quaternions])-- # Start with an initial guess (the first quaternion)- mean_quat = quaternions[0]-- for _ in range(max_iterations):- # Compute the tangent vectors (logarithm map)- errors = [R.from_quat(q).inv() * R.from_quat(mean_quat) for q in quaternions]- errors = [e.as_rotvec() for e in errors]-- # Compute the average error vector- mean_error = np.mean(errors, axis=0)-- # Check for convergence- if np.linalg.norm(mean_error) < tol:- break-- # Update the mean (exponential map)- mean_rot = R.from_rotvec(mean_error) * R.from_quat(mean_quat)- mean_quat = mean_rot.as_quat(canonical=False)-- return mean_quat--class RotationVector(BaseModel):- vector: RotationVectorArray- reference_frame: str-- @property- def as_rotation_matrix(self) -> RotationMatrixArray:- return cv2.Rodrigues(self.vector)[0]-- @property- def as_quaternion(self) -> QuaternionArray:- return R.from_matrix(self.as_rotation_matrix).as_quat(canonical=False)-- @classmethod- def mean_from_rotation_vectors(cls, rotation_vectors: list["RotationVector"]) -> "RotationVector":- if not all([rv.reference_frame == rotation_vectors[0].reference_frame for rv in rotation_vectors]):- raise ValueError("All rotation vectors must have the same reference frame!")- quaternions = [rv.as_quaternion for rv in rotation_vectors]-- # Compute the Karcher mean of the quaternions- mean_quat = karcher_mean_quaternions(quaternions)-- # Convert the mean quaternion back to a rotation vector- mean_rotation = R.from_quat(mean_quat)- mean_rotation_vector = mean_rotation.as_rotvec()-- # Assuming all rotation vectors have the same reference frame- reference_frame = rotation_vectors[0].reference_frame-- return cls(vector=mean_rotation_vector,- reference_frame=reference_frame)----class TranslationVector(BaseModel):- vector: TranslationVectorArray- reference_frame: str-- @classmethod- def mean_from_translation_vectors(cls, translation_vectors: list["TranslationVector"]) -> "TranslationVector":- if not all([tv.reference_frame == translation_vectors[0].reference_frame for tv in translation_vectors]):- raise ValueError("All translation vectors must have the same reference frame!")- mean_translation_vector = np.mean([tv.vector for tv in translation_vectors], axis=0)-- # Assuming all rotation vectors have the same reference frame- reference_frame = translation_vectors[0].reference_frame-- return cls(vector=mean_translation_vector,- reference_frame=reference_frame)---class TransformationMatrix(BaseModel):- matrix: NDArray[Shape["4, 4"], np.float64]- reference_frame: str-- @classmethod- def from_rotation_translation(cls,- rotation_vector: RotationVector,- translation_vector: TranslationVector):- if rotation_vector.reference_frame != translation_vector.reference_frame:- raise ValueError("Rotation and translation vectors must be in the same reference frame")- rotation_matrix = rotation_vector.as_rotation_matrix- transformation_matrix = np.eye(4)- transformation_matrix[:3, :3] = rotation_matrix- transformation_matrix[:3, 3] = translation_vector.vector- return cls(matrix=transformation_matrix, reference_frame=translation_vector.reference_frame)-- @classmethod- def mean_from_transformation_matrices(cls, transformation_matrices: list[- "TransformationMatrix"]) -> "TransformationMatrix":- if not all(- [tm.reference_frame == transformation_matrices[0].reference_frame for tm in transformation_matrices]):- raise ValueError("All transformation matrices must have the same reference frame!")-- rotation_vectors = [tm.rotation_vector for tm in transformation_matrices]- translation_vectors = [tm.translation_vector for tm in transformation_matrices]-- mean_rotation_vector = RotationVector.mean_from_rotation_vectors(rotation_vectors)- mean_translation_vector = TranslationVector.mean_from_translation_vectors(translation_vectors)-- return cls.from_rotation_translation(rotation_vector=mean_rotation_vector,- translation_vector=mean_translation_vector)-- @property- def rotation_matrix(self) -> NDArray[Shape["3, 3"], np.float32]:- return self.matrix[:3, :3]-- @property- def translation_vector(self) -> TranslationVector:- return TranslationVector(vector=self.matrix[:3, 3], reference_frame=self.reference_frame)-- @property- def rotation_vector(self) -> RotationVector:- return RotationVector(vector=np.squeeze(cv2.Rodrigues(self.rotation_matrix)[0]), reference_frame=self.reference_frame)-- @property- def as_extrinsics_matrix(self) -> CameraExtrinsicsMatrix:- return self.matrix[:3, :]--- def get_inverse(self):- inverse_rotation_matrix = self.rotation_matrix.T- inverse_translation_vector = -inverse_rotation_matrix @ self.translation_vector.vector-- inverse_matrix = np.eye(4)- inverse_matrix[:3, :3] = inverse_rotation_matrix- inverse_matrix[:3, 3] = inverse_translation_vector-- return TransformationMatrix(matrix=inverse_matrix, reference_frame=self.reference_frame)-- def __matmul__(self, other: "TransformationMatrix") -> "TransformationMatrix":- if not isinstance(other, TransformationMatrix):- raise TypeError(- "Unsupported operand type(s) for @: 'TransformationMatrix' and '{}'".format(type(other).__name__))- # Perform matrix multiplication and use self's reference frame- return TransformationMatrix(matrix=self.matrix @ other.matrix, reference_frame=self.reference_frame)--- def __str__(self) -> str:- return (f"\treference_frame={self.reference_frame},\n"- f"\tmatrix=\n"- f"\t\t{self.matrix[0, :]:.3f},\n"- f"\t\t{self.matrix[1, :]:.3f},\n"- f"\t\t{self.matrix[2, :]:.3f},\n"- f"\t\t{self.matrix[3, :]:.3f},\n")---class CameraDistortionCoefficients(BaseModel):- """- https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html- Distortion coefficients for the camera, must be either 4, 5, 8, 12, or 14 values.-- 0-3 are k1, k2, p1, p2- 4 is k3- 5-7 are k4, k5, k6- 8-11 are s1, s2, s3, s4- 12-13 are Ο1, Ο2 (Ο like tau)-- `k` values refer to radial distortion coefficients- `p` values refer to tangential distortion coefficients- `s` values refer to thin prism distortion coefficients- `Ο` values refer to x/y values of the 'tilted sensor' model-- NOTE - RECOMMEND USING 5 VALUES, things get weird with more than 5- """- coefficients: CameraDistortionCoefficientsArray-- @model_validator(mode="after")- def validate(self):- if len(self.coefficients) not in [4, 5, 8, 12, 14]:- raise ValueError("Invalid number of distortion coefficients. Must be 4, 5, 8, 12, or 14.")- return self---class CameraMatrix(BaseModel):- matrix: CameraMatrixArray-- @classmethod- def from_image_size(cls, image_size: tuple[int, ...]):- camera_matrix = np.eye(3)- camera_matrix[0, 2] = image_size[0] / 2 # x_center- camera_matrix[1, 2] = image_size[1] / 2 # y_center- return cls(matrix=camera_matrix)-- @model_validator(mode="after")- def validate(self):- if self.matrix.shape != (3, 3):- raise ValueError("Camera matrix must be 3x3")- return self-- @property- def focal_length(self) -> float:- return self.matrix[0, 0]-- @property- def focal_length_x(self) -> float:- return self.matrix[0, 0]-- @property- def focal_length_y(self) -> float:- return self.matrix[1, 1]-- @property- def focal_length_xy(self) -> float:- return (self.focal_length_x + self.focal_length_y) / 2-- @property- def principal_point(self) -> tuple[float, float]:- return self.matrix[0, 2], self.matrix[1, 2]--DEFAULT_INTRINSICS_COEFFICIENTS_COUNT = 5MIN_CHARUCO_CORNERS = 6@@ -265,7 +45,7 @@ class SingleCameraCalibrator(BaseModel):charuco_observations: CharucoObservations = Field(default_factory=CharucoObservations)object_points_views: list[ObjectPoints3D] = []- image_points_views: list[PixelPoints2D] = []+ image_points_views: list[ImagePoints2D] = []rotation_vectors: list[RotationVector] = []translation_vectors: list[TranslationVector] = []diff --git a/freemocap/pipelines/calibration_pipeline/triangulate_points.py b/freemocap/pipelines/calibration_pipeline/triangulate_points.pynew file mode 100644index 00000000..b9f266f9--- /dev/null+++ b/freemocap/pipelines/calibration_pipeline/triangulate_points.py@@ -0,0 +1,117 @@+import cv2+import numpy as np+from skellycam import CameraId++from freemocap.pipelines.calibration_pipeline.calibration_numpy_types import ObjectPoint3D, \+ ImagePoints2D, ObjectPoints3D, ReprojectionError, CameraExtrinsicsMatrix, ImagePoint2D+from freemocap.pipelines.calibration_pipeline.camera_math_models import TransformationMatrix+from freemocap.pipelines.calibration_pipeline.single_camera_calibrator import CameraIntrinsicsEstimate+++def undistort_points(points2d: ImagePoints2D,+ camera_intrinsics: CameraIntrinsicsEstimate) -> ImagePoints2D:+ """+ 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 (or attempts to ensure) 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) representing the 2D points+ to be undistorted, where N is the number of 2d points. 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 (in normalized image coordinates, I think?).+ """+ undistorted_points2d = cv2.undistortPoints(src=points2d,+ cameraMatrix= camera_intrinsics.camera_matrix.matrix,+ distCoeffs=camera_intrinsics.distortion_coefficients.coefficients,+ )++ return np.squeeze(undistorted_points2d)+++# @jit(nopython=True, parallel=False)+def triangulate_point(image_point_by_camera: dict[CameraId, ImagePoint2D],+ camera_extrinsics: dict[CameraId, CameraExtrinsicsMatrix]) -> ObjectPoint3D:+ _validate_triangulation_input(camera_extrinsics=camera_extrinsics,+ image_point_by_camera=image_point_by_camera)+ camera_ids = list(camera_extrinsics.keys())+ number_of_cameras = len(camera_ids)+ A = np.zeros((number_of_cameras * 2, 4)) #SVD needs this to be a 2d array of shape (2n, 4), where n is the number of cameras+ for camera_number, camera_id in enumerate(camera_ids):+ image_points_x = image_point_by_camera[camera_id][0]+ image_points_y = image_point_by_camera[camera_id][1]+ camera_matrix = camera_extrinsics[camera_id]+ A[(camera_number * 2): (camera_number * 2 + 1)] = image_points_x * camera_matrix[2] - camera_matrix[0]+ A[(camera_number * 2 + 1): (camera_number * 2 + 2)] = image_points_y * camera_matrix[2] - camera_matrix[1]+ u, s, vh = np.linalg.svd(A, full_matrices=True) #SVD returns the singular values in descending order, so the last row of vh is the solution (i.e. the 3D point represented in homogeneous coordinates)+ points_3d = vh[-1]+ points_3d = points_3d[:3] / points_3d[3]+ return points_3d+++def _validate_triangulation_input(camera_extrinsics: dict[CameraId, CameraExtrinsicsMatrix],+ image_point_by_camera: dict[CameraId, ImagePoint2D]):+ if len(image_point_by_camera) < 2:+ raise ValueError("Need at least two cameras to triangulate points.")+ if image_point_by_camera.keys() != camera_extrinsics.keys():+ raise ValueError("Camera IDs in image_point_by_camera and camera_extrinsics must match.")+ if not all([image_point_by_camera[camera_id].shape == (2,) for camera_id in image_point_by_camera.keys()]):+ raise ValueError("All image points must be 2D points.")+ if not all([camera_extrinsics[camera_id].shape == (3, 4)for camera_id in camera_extrinsics.keys()]):+ raise ValueError("All camera extrinsics must be 3x4 matrices.")+++def _validate_reprojection_error_input(object_points: ObjectPoints3D,+ image_points_by_camera: dict[CameraId, ImagePoints2D],+ camera_intrinsics: dict[CameraId, CameraIntrinsicsEstimate],+ camera_transforms: dict[CameraId, TransformationMatrix],+ image_sizes: dict[CameraId, tuple[int, int]]):+ if len(image_points_by_camera) < 2:+ raise ValueError("Need at least two cameras to triangulate points.")+ if any([image_sizes.keys() != camera_intrinsics.keys(),+ image_sizes.keys() != camera_transforms.keys(),+ image_sizes.keys() != image_points_by_camera.keys()]):+ raise ValueError("Camera IDs in image_points_by_camera, camera_intrinsics, and camera_transforms must match.")+ for camera_id in image_points_by_camera.keys():+ if not all ([point.shape == (2,) for point in image_points_by_camera[camera_id]]):+ raise ValueError("All image points must be 2D points.")++++# @jit(nopython=True, parallel=False)+def calculate_reprojection_error(object_points: ObjectPoints3D,+ image_points_by_camera: dict[CameraId, ImagePoints2D],+ camera_intrinsics: dict[CameraId, CameraIntrinsicsEstimate],+ camera_transforms: dict[CameraId, TransformationMatrix],+ image_sizes: dict[CameraId, tuple[int, int]]) -> tuple[list[ReprojectionError], dict[CameraId, list[ReprojectionError]]]:+ # https://docs.opencv.org/4.10.0/d9/d0c/group__calib3d.html#ga1019495a2c8d1743ed5cc23fa0daff8c+ _validate_reprojection_error_input(object_points=object_points,+ image_points_by_camera=image_points_by_camera,+ camera_intrinsics=camera_intrinsics,+ camera_transforms=camera_transforms,+ image_sizes=image_sizes)+ reprojection_error_per_point_by_camera: dict[CameraId, list[ReprojectionError]] = {}+ for camera_id in camera_intrinsics.keys():+ projected_image_points, jacobian = cv2.projectPoints(objectPoints=object_points,+ rvec=camera_transforms[camera_id].rotation_vector.vector,+ tvec=camera_transforms[+ camera_id].translation_vector.vector,+ cameraMatrix=camera_intrinsics[+ camera_id].camera_matrix.matrix,+ distCoeffs=camera_intrinsics[+ camera_id].distortion_coefficients.coefficients,+ aspectRatio=image_sizes[camera_id][0] / image_sizes[camera_id][1]+ )++ reprojection_error_per_point_by_camera[camera_id]= np.abs(image_points_by_camera[camera_id] - np.squeeze(projected_image_points))++ mean_reprojection_error_per_point = []+ for point_index in range(len(object_points)):+ point_reprojection_error = []+ for camera_id in camera_intrinsics.keys():+ point_reprojection_error.append(reprojection_error_per_point_by_camera[camera_id][point_index])+ mean_reprojection_error_per_point.append(float(np.nanmean(point_reprojection_error)))++ return mean_reprojection_error_per_point, reprojection_error_per_point_by_cameradiff --git a/freemocap/pipelines/mocap_pipeline/point_trangulator.py b/freemocap/pipelines/mocap_pipeline/point_trangulator.pyindex 9ac8b35d..21824229 100644--- a/freemocap/pipelines/mocap_pipeline/point_trangulator.py+++ b/freemocap/pipelines/mocap_pipeline/point_trangulator.py@@ -140,20 +140,20 @@ def _validate_input(points2d_by_camera: np.ndarray, camera_extrinsic_matricies:if camera_extrinsic_matricies.shape[1:] != (3, 4):raise ValueError(f"Expected camera extrinsic matricies to be of shape (num_cams, 3, 4), got {camera_extrinsic_matricies.shape}")-# @jit(nopython=True, parallel=False)-def triangulate_simple(points2d_by_camera: np.ndarray, camera_extrinsic_matricies: np.ndarray) -> tuple:- number_of_cameras, points_dimensions = points2d_by_camera.shape- _validate_input(points2d_by_camera, camera_extrinsic_matricies)- A = np.zeros((number_of_cameras * 2, 4))- for camera_number in range(number_of_cameras):- x, y = points2d_by_camera[camera_number]- camera_matrix = camera_extrinsic_matricies[camera_number]- A[(camera_number * 2) : (camera_number * 2 + 1)] = x * camera_matrix[2] - camera_matrix[0]- A[(camera_number * 2 + 1) : (camera_number * 2 + 2)] = y * camera_matrix[2] - camera_matrix[1]- u, s, vh = np.linalg.svd(A, full_matrices=True)- points_3d = vh[-1]- points_3d = points_3d[:3] / points_3d[3]- return tuple(points_3d)+# # @jit(nopython=True, parallel=False)+# def triangulate_simple(points2d_by_camera: np.ndarray, camera_extrinsic_matricies: np.ndarray) -> tuple:+# number_of_cameras, points_dimensions = points2d_by_camera.shape+# _validate_input(points2d_by_camera, camera_extrinsic_matricies)+# A = np.zeros((number_of_cameras * 2, 4))+# for camera_number in range(number_of_cameras):+# x, y = points2d_by_camera[camera_number]+# camera_matrix = camera_extrinsic_matricies[camera_number]+# A[(camera_number * 2) : (camera_number * 2 + 1)] = x * camera_matrix[2] - camera_matrix[0]+# A[(camera_number * 2 + 1) : (camera_number * 2 + 2)] = y * camera_matrix[2] - camera_matrix[1]+# u, s, vh = np.linalg.svd(A, full_matrices=True)+# points_3d = vh[-1]+# points_3d = points_3d[:3] / points_3d[3]+# return tuple(points_3d)if __name__ == "__main__":_point_triangulator = PointTriangulator.create()