Skip to content

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.py
index 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 CameraId
from tqdm import trange
from freemocap.pipelines.calibration_pipeline.calibration_numpy_types import \
- PixelPoints2DByCamera, CameraExtrinsicsMatrixByCamera
+ ImagePoints2DByCamera, CameraExtrinsicsMatrixByCamera
logger = 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.py
index 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 = None
data: 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 in
input_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.py
index 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 np
from 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 coordinates
CameraDistortionCoefficientsArray = 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 translation
IntrinsicsParameters = 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, var
ExtrinsicsParametersByCamera = 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 file
diff --git a/freemocap/pipelines/calibration_pipeline/camera_math_models.py b/freemocap/pipelines/calibration_pipeline/camera_math_models.py
new file mode 100644
index 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.py
index 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 np
from pydantic import BaseModel
+from pydantic import model_validator
from scipy import optimize
from scipy.sparse import dok_matrix
from skellycam import CameraId
+from typing_extensions import Self
from freemocap.pipelines.calibration_pipeline.calibration_numpy_types import \
- JacobianMatrixArray
+ ImagePoints2D, ImagePoint2D, CameraExtrinsicsMatrix
+from freemocap.pipelines.calibration_pipeline.camera_math_models import TransformationMatrix
from 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_guess
def optimize(self) -> optimize.OptimizeResult:
+
# Set up optimization options
return 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 from
x_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.py
index 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 trange
from 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, TranslationVectorsByCamera
from 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_views
def 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_plane
class MultiCameraCalibrationInputData(BaseModel):
- pixel_points2d: PixelPoints2DByCamera
+ pixel_points2d: ImagePoints2DByCamera
object_points3d: ObjectPoints3D
points_ids: PointIds
rotation_vectors: RotationVectorsByCamera
diff --git a/freemocap/pipelines/calibration_pipeline/multi_camera_calibrator.py b/freemocap/pipelines/calibration_pipeline/multi_camera_calibrator.py
index 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 Sha
logger = logging.getLogger(__name__)
from freemocap.pipelines.calibration_pipeline.single_camera_calibrator import SingleCameraCalibrator, \
- TransformationMatrix
+ CameraIntrinsicsEstimate
+from freemocap.pipelines.calibration_pipeline.camera_math_models import TransformationMatrix
class 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 in
self.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 in
self.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.py
index 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 np
from pydantic import BaseModel, Field
from skellycam import CameraId
from pydantic import model_validator
from freemocap.pipelines.calibration_pipeline.calibration_camera_node_output_data import CalibrationCameraNodeOutputData
-
+from freemocap.pipelines.calibration_pipeline.calibration_numpy_types import ImagePoint2D
MultiFrameNumber = int
class 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 = int
class MultiCameraTargetView(BaseModel):
multi_frame_number: MultiFrameNumber
camera_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.py
index 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 logging
import cv2
import numpy as np
-from numpydantic import NDArray, Shape
-from pydantic import BaseModel, Field, model_validator
+from pydantic import BaseModel, Field
from skellycam import CameraId
from skellytracker.trackers.charuco_tracker.charuco_observation import CharucoObservations, CharucoObservation, \
AllCharucoCorners3DByIdInObjectCoordinates, AllArucoCorners3DByIdInObjectCoordinates
from 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, CameraMatrix
logger = 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 = 5
MIN_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.py
new file mode 100644
index 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_camera
diff --git a/freemocap/pipelines/mocap_pipeline/point_trangulator.py b/freemocap/pipelines/mocap_pipeline/point_trangulator.py
index 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()